//***********************************************************
// 26/01/2021	1.0.0	Rémi Saint-Amant   Creation
//***********************************************************
#include "ALeucopodaModel.h"
//#include "ALeucopodaEquations.h"
#include "ModelBase/EntryPoint.h"
#include "Basic\DegreeDays.h"
//#include "Leucopoda.h"
//#include <boost/math/distributions/weibull.hpp>
//#include <boost/math/distributions/beta.hpp>
//#include <boost/math/distributions/Rayleigh.hpp>
#include <boost/math/distributions/logistic.hpp>
//#include <boost/math/distributions/exponential.hpp>
//#include <boost/math/distributions/lognormal.hpp>
#include "ModelBase/SimulatedAnnealingVector.h"

using namespace WBSF::HOURLY_DATA;
using namespace std;
using namespace WBSF::TZZ;

//static const bool BEGIN_JULY = true;
//static const size_t FIRST_Y = BEGIN_JULY ? 1 : 0;

namespace WBSF
{

	static const size_t NB_GENERATIONS_MAX = 6;

	//static const CDegreeDays::TDailyMethod DD_METHOD = CDegreeDays::MODIFIED_ALLEN_WAVE;
	static const CDegreeDays::TDailyMethod DD_METHOD = CDegreeDays::ALLEN_WAVE;
	//enum { O_CDD, O_GENERATION, O_DIAPAUSED, O_EGG, O_LARVA, O_PUPA, O_ADULT, O_DEAD_ADULT, O_BROOD, NB_OUTPUTS };
	enum { O_EGG, O_LARVA, O_PREPUPA, O_PUPA, O_ADULT, O_DEAD_ADULT, O_BROOD, O_DEAD_ATTRITION, NB_OUTPUT_ONE_G, O_IN_DIAPAUSE = (NB_OUTPUT_ONE_G * NB_GENERATIONS_MAX - O_PUPA), O_D_DAY_LENGTH, NB_DAILY_OUTPUTS };

	//this line link this model with the EntryPoint of the DLL
	static const bool bRegistred =
		CModelFactory::RegisterModel(CAprocerosLeucopodaModel::CreateObject);

	CAprocerosLeucopodaModel::CAprocerosLeucopodaModel()
	{
		//NB_INPUT_PARAMETER is used to determine if the dll
		//uses the same number of parameters than the model interface
		NB_INPUT_PARAMETER = -1;
		VERSION = "1.0.0 (2021)";

		m_generationSurvival = 1;
//		m_bCumul = false;
//		m_stage = 0;
	//	m_T = 0;

		//m_EWD.fill(0);
		//m_EAS.fill(0);
		//Set parameters to equation
		//ASSERT(stand.m_equations.m_EWD.size() == m_EWD.size());
		for (size_t p = 0; p < m_EWD.size(); p++)
			m_EWD[p] = CAprocerosLeucopodaEquations::EWD[p];

		//ASSERT(stand.m_equations.m_EAS.size() == m_EAS.size());
		for (size_t p = 0; p < m_EAS.size(); p++)
			m_EAS[p] = CAprocerosLeucopodaEquations::EAS[p];

		//m_P.fill(0);
		/*m_P[Τᴴ²] = 19.1;
		m_P[delta] = 45;
		m_P[μ1] = 239.6;
		m_P[ѕ1] = 41.4;
		m_P[μ2] = 876.0;
		m_P[ѕ2] = 55.6;
		m_P[μ3] = 625.2;
		m_P[ѕ3] = 38.3;
			*/
	}

	CAprocerosLeucopodaModel::~CAprocerosLeucopodaModel()
	{
	}

	//this method is call to load your parameter in your variable
	ERMsg CAprocerosLeucopodaModel::ProcessParameters(const CParameterVector& parameters)
	{
		ERMsg msg;

		size_t c = 0;

		//		m_stage = parameters[c++].GetInt() + 1;
			//	m_T = parameters[c++].GetInt()+1;

		//m_bCumul = parameters[c++].GetBool();
		if (parameters.size() == m_EWD.size() + m_EAS.size() + 1)
		{
			//entering winter diapause  parameters
			for (size_t p = 0; p < m_EWD.size(); p++)
			{
				m_EWD[p] = parameters[c++].GetFloat();
			}

			//Emerging Adult from Soil (spring) parameters
			for (size_t p = 0; p < m_EAS.size(); p++)
			{
				m_EAS[p] = parameters[c++].GetFloat();
			}
		}

		return msg;
	}

	ERMsg CAprocerosLeucopodaModel::OnExecuteDaily()
	{
		ERMsg msg;

		/*	if (m_weather.GetNbYears() < 2)
			{
				msg.ajoute("Laricobius nigrinus model need at least 2 years of data");
				return msg;
			}*/

		if (!m_weather.IsHourly())
			m_weather.ComputeHourlyVariables();

		//This is where the model is actually executed
		m_output.Init(m_weather.GetEntireTPeriod(CTM(CTM::DAILY)), NB_DAILY_OUTPUTS, 0);

		//we simulate 2 years at a time.
		//we also manager the possibility to have only one year
		for (size_t y = 0; y < m_weather.size(); y++)
		{
			//one output by generation
			vector<CModelStatVector> outputs;
			ExecuteDaily(m_weather[y].GetTRef().GetYear(), m_weather, outputs);

			//merge generations vector into one output vector (max of 5 generations)
			size_t maxG = min(NB_GENERATIONS_MAX, outputs.size());

			CTPeriod p = m_weather[y].GetEntireTPeriod(CTM(CTM::DAILY));
			for (CTRef TRef = p.Begin(); TRef <= p.End(); TRef++)
			{
				CStatistic diapause;
				for (size_t g = 0, ss = 0; g < maxG; g++)
				{
					size_t s_i = (g==0)?PUPA:EGG;
					for (size_t s = s_i; s < NB_OUTPUT_ONE_G; s++, ss++)
					{
						m_output[TRef][ss] = outputs[g][TRef][s];
						diapause += outputs[g][TRef][S_DIAPAUSE];
					}
				}

				m_output[TRef][O_IN_DIAPAUSE] = diapause[SUM];
				m_output[TRef][O_D_DAY_LENGTH] = m_weather.GetDayLength(TRef) / 3600.;
			}
		}

		return msg;
	}

	void CAprocerosLeucopodaModel::ExecuteDaily(int year, const CWeatherYears& weather, vector<CModelStatVector>& output)
	{
		//Create stand
		CTZZStand stand(this);
		stand.m_bApplyAttrition = true;
		stand.m_generationSurvival = m_generationSurvival;

		//Set parameters to equation

		ASSERT(stand.m_equations.m_EWD.size() == m_EWD.size());
		for (size_t p = 0; p < m_EWD.size(); p++)
			stand.m_equations.m_EWD[p] = m_EWD[p];

		ASSERT(stand.m_equations.m_EAS.size() == m_EAS.size());
		for (size_t p = 0; p < m_EAS.size(); p++)
			stand.m_equations.m_EAS[p] = m_EAS[p];

		stand.init(year, weather);

		//compute 30 days avg
		//stand.ComputeTavg30(year, weather);

		//Create host
		CTZZHostPtr pHost(new CTZZHost(&stand));

		pHost->m_nbMinObjects = 10;
		pHost->m_nbMaxObjects = 1000;

		pHost->Initialize<CAprocerosLeucopoda>(CInitialPopulation(CTRef(year, JANUARY, DAY_01), 0, 400, 100, TZZ::PUPA, WBSF::FEMALE, true, 0));

		//add host to stand
		stand.m_host.push_front(pHost);

		CTPeriod p = weather[year].GetEntireTPeriod(CTM(CTM::DAILY));

		for (CTRef d = p.Begin(); d <= p.End(); d++)
		{

			stand.Live(weather.GetDay(d));
			//if (output.IsInside(d))
				//stand.GetStat(d, output[d]);

			size_t nbGenerations = stand.GetFirstHost()->GetNbGeneration();
			if (nbGenerations > output.size())
				output.push_back(CModelStatVector(p, NB_STATS, 0));

			for (size_t g = 0; g < nbGenerations; g++)
				stand.GetStat(d, output[g][d], g);

			stand.AdjustPopulation();
			HxGridTestConnection();
		}

		//if (m_bCumul)
		//{
		//	for (size_t g = 0; g < output.size(); g++)
		//	{
		//		//cumulative result
		//		for (size_t s = S_EGG; s < S_ADULT; s++)
		//		{
		//			CTPeriod p = weather[year].GetEntireTPeriod(CTM(CTM::DAILY));

		//			CStatistic stat = output[g].GetStat(s, p);
		//			if (stat.IsInit() && stat[SUM] > 0)
		//			{
		//				output[g][0][s] = output[g][0][s] * 100 / stat[SUM];//when first day is not 0
		//				for (CTRef d = p.Begin() + 1; d <= p.End(); d++)
		//				{
		//					output[g][d][s] = output[g][d - 1][s] + output[g][d][s] * 100 / stat[SUM];
		//					_ASSERTE(!_isnan(output[g][d][s]));
		//				}
		//			}
		//		}
		//	}
		//}
	}

	//void CAprocerosLeucopodaModel::GetCDD(int year, const CWeatherYears& weather, CModelStatVector& CDD)
	//{
	//	CDegreeDays DDmodel(DD_METHOD, m_P[Τᴴ¹], m_P[Τᴴ²]);
	//	CModelStatVector DD;
	//	DDmodel.Execute(weather[year], DD);

	//	CDD.Init(DD.GetTPeriod(), 1, 0.0);

	//	//for (CTRef TRef = DD.GetFirstTRef(); TRef <= DD.GetLastTRef(); TRef++)
	//	CDD[0][0] = DD[0][CDegreeDays::S_DD];
	//	for (size_t i = 1; i < DD.size(); i++)
	//		CDD[i][0] = CDD[i - 1][0] + DD[i][CDegreeDays::S_DD];
	//}

	void CAprocerosLeucopodaModel::GetCDD(const CWeatherYears& weather, CModelStatVector& CDD)
	{
		//CTZZStand* pStand = GetStand();
		CDegreeDays DDmodel(DD_METHOD, m_EAS[Τᴴ¹], m_EAS[Τᴴ²]);
		CModelStatVector DD;
		DDmodel.Execute(weather, DD);

		CDD.Init(DD.GetTPeriod(), 1, -999);

		for (size_t y = 0; y < weather.GetNbYears(); y++)
		{
			CTPeriod p = weather[y].GetEntireTPeriod();
			//p.Begin() = p.Begin() + int(m_P[delta]);
			CDD[p.Begin()][0] = DD[p.Begin()][CDegreeDays::S_DD];

			for (CTRef TRef = p.Begin() + 1; TRef <= p.End(); TRef++)
				CDD[TRef][0] = CDD[TRef - 1][0] + DD[TRef][CDegreeDays::S_DD];
		}

	}

	//enum TSpecies { S_LA_G1, S_LA_G2, S_LP, S_LN };
	enum TInput { I_KEYID, I_DATE, I_STAGE, I_GENERATION, I_N, I_CUMUL, NB_INPUTS };
	void CAprocerosLeucopodaModel::AddDailyResult(const StringVector& header, const StringVector& data)
	{
		//KeyID	Date	Stage	Generation	N	Cumul
		ASSERT(data.size() == NB_INPUTS);
		//SYC	site	Year	collection	col_date	emerge_date	daily_count	species	n_days P Time
		//if (stoi(data[I_VARIABLE]) == m_stage && stoi(data[I_T]) == m_T)
		//{
		CSAResult obs;
		obs.m_ref.FromFormatedString(data[I_DATE]);
		obs.m_obs.resize(NB_INPUTS);
		for (size_t i = 2; i < NB_INPUTS; i++)
		{
			if (i == I_STAGE)
				obs.m_obs[i] = 0;
			else
				obs.m_obs[i] = stod(data[i]);
		}

		//CSAResult obs;

		//CStatistic egg_creation_date;

		//obs.m_obs.resize(3);
		//obs.m_obs[0] = 0;
		//obs.m_obs[1] = stod(data[I_GENERATION]);
		//obs.m_obs[2] = stod(data[I_CUMUL]);
		////obs.m_obs[I_S] = ;
		////obs.m_obs[I_P] = stod(data[10]);
		////obs.m_obs[I_CDD] = stod(data[11]);
		////obs.m_obs[I_P] = stod(data[12]);

		//if (data[7] == "LA" && data[8] == "1")
		//	obs.m_obs[I_S] = S_LA_G1;
		//else if (data[7] == "LA" && data[8] == "2")
		//	obs.m_obs[I_S] = S_LA_G2;
		//else if (data[7] == "LP")
		//	obs.m_obs[I_S] = S_LP;
		//else if (data[7] == "LN")
		//	obs.m_obs[I_S] = S_LN;

		m_SAResult.push_back(obs);
	}

	//ASSERT(data.size() == NB_INPUTS);
	////SYC	site	Year	collection	col_date	emerge_date	daily_count	species	n_days P Time
	//if (stoi(data[I_VARIABLE]) == m_stage && stoi(data[I_T]) == m_T)
	//{
	//	CSAResult obs;
	//	obs.m_obs.resize(NB_INPUTS);
	//	for (size_t i = 0; i < NB_INPUTS; i++)
	//		obs.m_obs[i] = stod(data[i]);
	//	//CSAResult obs;

	//	//CStatistic egg_creation_date;

	//	//obs.m_ref.FromFormatedString(data[5]);
	//	//obs.m_obs.resize(NB_INPUTS);
	//	//obs.m_obs[I_N] = stod(data[6]);
	//	////obs.m_obs[I_S] = ;
	//	////obs.m_obs[I_P] = stod(data[10]);
	//	////obs.m_obs[I_CDD] = stod(data[11]);
	//	////obs.m_obs[I_P] = stod(data[12]);

	//	//if (data[7] == "LA" && data[8] == "1")
	//	//	obs.m_obs[I_S] = S_LA_G1;
	//	//else if (data[7] == "LA" && data[8] == "2")
	//	//	obs.m_obs[I_S] = S_LA_G2;
	//	//else if (data[7] == "LP")
	//	//	obs.m_obs[I_S] = S_LP;
	//	//else if (data[7] == "LN")
	//	//	obs.m_obs[I_S] = S_LN;

	//	m_SAResult.push_back(obs);
	//}
//	}

	//double GetSimX(size_t s, CTRef TRefO, double obs, const CModelStatVector& output)
	//{
	//	double x = -999;

	//	if (obs > -999)
	//	{
	//		//if (obs > 0.01 && obs < 99.99)
	//		if (obs >= 100)
	//			obs = 99.99;//to avoid some problem of truncation

	//		long index = output.GetFirstIndex(s, ">=", obs, 1, CTPeriod(TRefO.GetYear(), FIRST_MONTH, FIRST_DAY, TRefO.GetYear(), LAST_MONTH, LAST_DAY));
	//		if (index >= 1)
	//		{
	//			double obsX1 = output.GetFirstTRef().GetJDay() + index;
	//			double obsX2 = output.GetFirstTRef().GetJDay() + index + 1;

	//			double obsY1 = output[index][s];
	//			double obsY2 = output[index + 1][s];
	//			if (obsY2 != obsY1)
	//			{
	//				double slope = (obsX2 - obsX1) / (obsY2 - obsY1);
	//				double obsX = obsX1 + (obs - obsY1)*slope;
	//				ASSERT(!_isnan(obsX) && _finite(obsX));

	//				x = obsX;
	//			}
	//		}
	//	}

	//	return x;
	//}

	bool CAprocerosLeucopodaModel::IsParamValid()const
	{
		bool bValid = true;

		return bValid;
	}

	//static const int ROUND_VAL = 4;
	//CTRef CAprocerosLeucopodaModel::GetEmergence(const CWeatherYear& weather)
	//{
	//	CTPeriod p = weather.GetEntireTPeriod(CTM(CTM::DAILY));

	//	double sumDD = 0;
	//	for (CTRef TRef = p.Begin()+172; TRef <= p.End()&& TRef<= p.Begin() + int(m_ADE[ʎ0]); TRef++)
	//	{
	//		//size_t ii = TRef - p.Begin();
	//		const CWeatherDay& wday = m_weather.GetDay(TRef);
	//		double T = wday[H_TNTX][MEAN];
	//		T = Round(max(m_ADE[ʎa], T), ROUND_VAL);

	//		double DD = min(0.0, T - m_ADE[ʎb]);//DD is negative

	//		//if (ii < m_ADE[ʎ0])
	//			sumDD += DD;
	//	}

	//	boost::math::logistic_distribution<double> begin_dist(m_ADE[ʎ2], m_ADE[ʎ3]);
	//	int begin = (int)Round(m_ADE[ʎ0] + m_ADE[ʎ1] * cdf(begin_dist, sumDD), 0);
	//	return  p.Begin() + begin;
	//}
	//enum TPout {P_CDD, P_CE, LA_G1= P_CE, P_LA_G2, P_LP, P_LN, NB_P};//CE = cumulative emergence
	//void CAprocerosLeucopodaModel::GetPobs(CModelStatVector& P)
	//{
	//	string ID = GetInfo().m_loc.m_ID;
	//	string SY = ID.substr(0, ID.length() - 2);

	//	//compute CDD for all temperature rprofile
	//	array< double, 4> total = { 0 };
	//	vector<tuple<double, CTRef, double, bool, size_t>> d;
	//	const CSimulatedAnnealingVector& SA = GetSimulatedAnnealingVector();

	//	for (size_t i = 0; i < SA.size(); i++)
	//	{
	//		string IDi = SA[i]->GetInfo().m_loc.m_ID;
	//		string SYi = IDi.substr(0, IDi.length() - 2);
	//		if (SYi == SY)
	//		{
	//			CModelStatVector CDD;
	//			GetCDD(SA[i]->m_weather, CDD);
	//			const CSAResultVector& v = SA[i]->GetSAResult();
	//			for (size_t ii = 0; ii < v.size(); ii++)
	//			{
	//				d.push_back(make_tuple(CDD[v[ii].m_ref][0], v[ii].m_ref, v[ii].m_obs[I_N], IDi == ID, v[ii].m_obs[I_S]));
	//				total[v[ii].m_obs[I_S]] += v[ii].m_obs[I_N];
	//			}
	//		}
	//	}

	//	sort(d.begin(), d.end());

	//	P.Init(m_weather.GetEntireTPeriod(CTM::DAILY), NB_P, 0);
	//	array< double, 4> sum = { 0 };
	//	for (size_t i = 0; i < d.size(); i++)
	//	{
	//		size_t s = std::get<4>(d[i]);
	//		sum[s] += std::get<2>(d[i]);
	//		if (std::get<3>(d[i]))
	//		{
	//			CTRef Tref = std::get<1>(d[i]);
	//			/*double obsP = -999;
	//			for (size_t k = 0; k < m_SAResult.size(); k++)
	//				if (m_SAResult[k].m_ref == Tref)
	//					obsP = m_SAResult[k].m_obs[I_P];*/

	//			double CDD = std::get<0>(d[i]);
	//			double p = Round(100 * sum[s] / total[s], 1);
	//
	//			P[Tref][P_CDD] = CDD;
	//			P[Tref][P_CE + s] = p;
	//		}
	//	}
	//}

	//void CAprocerosLeucopodaModel::CalibrateEmergence(CStatisticXY& stat)
	//{
	//	if (m_SAResult.empty())
	//		return;

	//	//boost::math::lognormal_distribution<double> emerge_dist(m_P[μ], m_P[ѕ]);

	//	//boost::math::weibull_distribution<double> emerge_dist(m_P[μ], m_P[ѕ]);
	//	//boost::math::beta_distribution<double> emerge_dist(m_P[μ], m_P[ѕ]);
	//	//boost::math::exponential_distribution<double> emerge_dist(m_P[ѕ]);
	//	//boost::math::rayleigh_distribution<double> emerge_dist(m_P[ѕ]);

	//	//CModelStatVector CDD;
	//	//GetCDD(m_weather, CDD);

	//	double n = 0;

	//	for (size_t i = 0; i < m_SAResult.size(); i++)
	//		n += m_SAResult[i].m_obs[I_N];

	//	CModelStatVector P;
	//	GetPobs(P);

	//	//array<boost::math::logistic_distribution<double>,4> emerge_dist(mu, S);
	//	//		array<boost::math::logistic_distribution<double>, 4> emerge_dist = { {m_P[μ1], m_P[ѕ1], m_P[μ1], m_P[ѕ1]} };

	//	for (size_t i = 0; i < m_SAResult.size(); i++)
	//	{
	//		size_t s = m_SAResult[i].m_obs[I_S];
	//		double mu = m_P[μ1 + 2 * s];
	//		double S = m_P[ѕ1 + 2 * s];

	//		boost::math::logistic_distribution<double> emerge_dist(mu, S);

	//		double CDD = P[m_SAResult[i].m_ref][P_CDD];
	//		double obs = P[m_SAResult[i].m_ref][P_CE+s];
	//		ASSERT(obs >= 0 && obs <= 100);

	//		double sim = Round(100 * cdf(emerge_dist, max(0.0, CDD)), 1);
	//		for (size_t ii = 0; ii < log(n); ii++)
	//			stat.Add(obs, sim);

	//	}//for all results

	//	return;

	//}

	//static double ei(size_t n) { return pow(1.0 + 1.0 / n, n); }
	//static double cv_2_sigma(double cv, size_t n)
	//{
	//	static const double e = exp(1);
	//	static const double p[3] = { 0.528196, 2.373248, 3.493202 };//with 10 000 replication
	//	return e * cv*(1 - p[0] * sqrt(e - ei(n))) / (p[1] + cv * (1 - p[2] * sqrt(e - ei(n))));
	//}

	void CAprocerosLeucopodaModel::GetFValueDaily(CStatisticXY& stat)
	{

		ASSERT(!m_SAResult.empty() );

		//CTZZStand* pStand = GetStand();
		//for (size_t p = 0; p < m_EAS.size(); p++)
			//pStand->m_equations.m_EAS[p] = m_EAS[p];

		if (!IsParamValid())
			return;

		//boost::math::lognormal_distribution<double> emerge_dist(m_P[μ], m_P[ѕ]);

		//boost::math::weibull_distribution<double> emerge_dist(m_P[μ], m_P[ѕ]);
		//boost::math::beta_distribution<double> emerge_dist(m_P[μ], m_P[ѕ]);
		//boost::math::exponential_distribution<double> emerge_dist(m_P[ѕ]);
		//boost::math::rayleigh_distribution<double> emerge_dist(m_P[ѕ]);

		//CModelStatVector CDD;
		//GetCDD(m_weather, CDD);

		double n = 0;

		for (size_t i = 0; i < m_SAResult.size(); i++)
			n += m_SAResult[i].m_obs[I_N];

		CModelStatVector CDD;
		GetCDD(m_weather, CDD);
		//GetPobs(P);

		boost::math::logistic_distribution<double> emerge_dist(m_EAS[μ], m_EAS[ѕ]);

		for (size_t i = 0; i < m_SAResult.size(); i++)
		{
			size_t s = m_SAResult[i].m_obs[I_STAGE];

			double GDD = CDD[m_SAResult[i].m_ref][CDegreeDays::S_DD];
			double obs = m_SAResult[i].m_obs[I_CUMUL];
			//ASSERT(obs >= 0 && obs <= 100);

			double sim = Round(100 * cdf(emerge_dist, max(0.0, GDD)), 1);
			//for (size_t ii = 0; ii < log(n); ii++)
			stat.Add(obs, sim);

		}//for all results

		return;

		//bool bZero = false;
		//CStatistic x;
		//for (size_t i = 0; i < m_SAResult.size(); i++)
		//{
		//	//ASSERT(m_SAResult[i].m_obs[I_VARIABLE] == m_stage);
		//	//ASSERT(m_SAResult[i].m_obs[I_T] == m_T);

		//	size_t n = size_t(ceil(m_P[i]));
		//	bZero |= n == 0;

		//	for (size_t b = 0; b < n; b++)
		//		x += m_SAResult[i].m_obs[I_TIME];

		//}//for all results

		////if (!bZero  && int(x[NB_VALUE]) == int(m_SAResult[0].m_obs[I_N]) )
		//{
		//	double obs = m_SAResult[0].m_obs[I_MEAN_TIME];
		//	double sim = x[MEAN];
		//	stat.Add(obs, sim);

		//	obs = m_SAResult[0].m_obs[I_TIME_SD] * 10;
		//	sim = x[STD_DEV] * 10;
		//	stat.Add(obs, sim);

		//	obs = size_t(m_SAResult[0].m_obs[I_N]);
		//	sim = size_t(x[NB_VALUE]);
		//	stat.Add(obs, sim);

		//	CStatistic stat_ws;
		//	CStatistic stat_n;

		//	//compute sigma

		//	double mean = x[MEAN];
		//	double sd = x[STD_DEV];
		//	double n = x[NB_VALUE];
		//	if (n > 0)
		//	{
		//		double cv = sd / mean;
		//		double sigma = cv_2_sigma(cv, n);

		//		stat_ws += n * sigma;
		//		stat_n += n;
		//	}

		//	static const double SIGMA_OBS[4] = { 0.102, 0.157, 0.203, 0.118 };

		//	obs = SIGMA_OBS[m_stage];
		//	sim = stat_ws[SUM] / stat_n[SUM];

		//}

	}

	//void CAprocerosLeucopodaModel::FinalizeStat(CStatisticXY& stat)
	//{
	//
	//}
}


//State Vector- variable acceleration model
	// X(0) = quad_position.point.x;
    // X(1) = m_velocity.point.x;
    // X(2) = m_acc_bias.point.x;
    // X(3) = quad_position.point.y;
    // X(4) = m_velocity.point.y;
    // X(5) = m_acc_bias.point.y;
    // X(6) = quad_position.point.z;
    // X(7) = m_velocity.point.z;
    // X(8) = m_acc_bias.point.z;
	// X(9)  = leader.point.x;
    // X(10) = leader.point.y;
    // X(11) = leader.point.z;
	// X(12) = leader.point.x;
	// X(13) = leader.point.y;
	// X(14) = leader.point.z;
//Input Vector
	// U(0) = quad.imu.acc.x
	// U(1) = 0
	// U(2) = 0
	// U(3) = quad.imu.acc.y
	// U(4) = 0
	// U(5) = 0
	// U(6) = quad.imu.acc.z
	// U(7) = 0
	// U(8) = 0
	// U(9)  = leader_quad.point.x
	// U(10) = leader_quad.point.y
	// U(11) = leader_quad.point.z
	// U(12) = neighbor_quad.point.x
	// U(13) = neighbor_quad.point.y
	// U(14) = neighbor_quad.point.z

#include <Eigen/Dense>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <bits/stdc++.h>
#include <ros/ros.h>
#include <math.h>
#include <stdio.h>
#include <dwm1001/anchor.h>
#include <sensor_msgs/Imu.h>
#include "std_msgs/MultiArrayLayout.h"
#include "std_msgs/MultiArrayDimension.h"
#include "std_msgs/Float32MultiArray.h"
#include "geometry_msgs/PoseStamped.h"
#include "geometry_msgs/Vector3Stamped.h"

// Declarations
int count =0 ;
double r_pred_l,r_meas_l;
Eigen::MatrixXd H_l(1,15);
double R_l;
double m_R_scale_l,sigma_r_l;
Eigen::VectorXd K_l(15);
Eigen::VectorXd X_p_l(15);
Eigen::MatrixXd P_p_l(15,15);
double error_l;
double precisionRangeErrEst_l;
//
double r_pred_n,r_meas_n;
Eigen::MatrixXd H_n(1,15);
double R_n;
double m_R_scale_n,sigma_r_n;
Eigen::VectorXd K_n(15);
Eigen::VectorXd X_p_n(15);
Eigen::MatrixXd P_p_n(15,15);
double error_n;
double precisionRangeErrEst_n;
//
geometry_msgs::Vector3Stamped pose;
geometry_msgs::Vector3Stamped vel;
geometry_msgs::Vector3Stamped acc_;
float ax,ay,az;
Eigen::Quaternionf q;
Eigen::MatrixXf R_mat(3,3);
Eigen::MatrixXf imuacc(3,1);
Eigen::MatrixXf acc(3,1);
//
Eigen::VectorXd X(15);
Eigen::VectorXd X_e(15);
Eigen::MatrixXd F(15,15);
Eigen::MatrixXd block_F(3,3);
Eigen::MatrixXd Q(15,15);
Eigen::VectorXd u(15);
Eigen::MatrixXd B(15,15);
Eigen::MatrixXd P(15,15);
Eigen::MatrixXd M(15,15);
Eigen::MatrixXd block_B(3,3);
Eigen::MatrixXd block_Q(3,3);
Eigen::MatrixXd B_I(6,6);
Eigen::MatrixXd Q_lead(3,3);
Eigen::MatrixXd Q_neigh(3,3);
double tao_acc;
double tao_bias;
double sigma_a,r_meas;
double T;
double m_last_range_time;
std_msgs::Float32MultiArray output;
double error_threshold,precisionRangeMm;
double m_kalman_sigma_a,T_sq,m_tao_acc_sqrt,m_tao_bias_sqrt,T_cub,m_z_damping_factor,m_Q_scale;
double m_snr_threshold,error;
double x,y,z,x_l,y_l,z_l,x_n,y_n,z_n;
double q_l,q_n;

void setState(Eigen::VectorXd Y)
{
    X = Y;
}
void setCovariance(Eigen::MatrixXd S)
{
    P = S;
}

void convert_NED(sensor_msgs::Imu imu)
{
	//conerts the acceleration from body frame to earth frame(NED)
    q = Eigen::Quaternionf(imu.orientation.w, imu.orientation.x, imu.orientation.y, imu.orientation.z);
    R_mat= q.toRotationMatrix();

    ax=imu.linear_acceleration.x;
    ay=imu.linear_acceleration.y;
    az=imu.linear_acceleration.z;
    imuacc << ax,ay,az;
    acc= R_mat*imuacc;

}

void prediction_step(const sensor_msgs::Imu::ConstPtr& msg)
{
	convert_NED(*msg);
  	ax=acc(0,0);
  	ay=-acc(1,0);
  	az=-1*(acc(2,0)-9.8);

    //ROS_WARN("Acceleration:## %f, ## %f, ## %f", ax,ay,az);
    T = msg->header.stamp.toSec() - m_last_range_time;
    if(T>1){
        T = 1;
    } else if(T<0){
        T = 0.01;
    }

    sigma_a = m_kalman_sigma_a;
   // r_meas = double(precisionRangeMm) / 1000.0;

    T_sq = std::pow(T,2);
    T_cub = std::pow(T,3);

    // F is a 9x9 State Transition Matrix
    F = Eigen::MatrixXd::Zero(15,15);

    block_F << 1, T, -T_sq/2.0,
               0, 1, -T,
               0, 0, 1;
    F.block<3,3>(0,0) = block_F;
    F.block<3,3>(3,3) = block_F;
    F.block<3,3>(6,6) = block_F;

    u << ax, 0, 0,
         ay, 0, 0,
         az, 0, 0,
         x_l,y_l,z_l,
         x_n,y_n,z_n;
    B = Eigen::MatrixXd::Zero(15,15);
    B_I = Eigen::MatrixXd::Identity(6,6);
    block_B << T_sq/2.0,  0,  0,
               T      ,  0,  0,
               0      ,  0,  0;
    B.block<3,3>(0,0) = block_B;
    B.block<3,3>(3,3) = block_B;
    B.block<3,3>(6,6) = block_B;
    B.block<6,6>(9,9) = B_I;

    // X is the predicted state vector and the predicted covariance matrix
    X_e = F*X + B * u;

    // Q is the acceleration model
    tao_acc = m_tao_acc_sqrt * m_tao_acc_sqrt;
    tao_bias = m_tao_bias_sqrt * m_tao_bias_sqrt;
    Q = Eigen::MatrixXd::Zero(15,15);

    block_Q << (T_cub*tao_acc/3.0)+(T_cub*T_sq)*tao_bias/20.0, (T_sq*tao_acc/2)+(T_sq*T_sq)*tao_bias/8.0  ,-T_cub*tao_bias/6,
        	   (T_sq*tao_acc/2.0)+(T_sq*T_sq)*tao_bias/8.0 ,   T*tao_acc+(T_cub*tao_bias/3) 			  ,-T_sq*tao_bias/2,
         	   -T_cub*tao_bias/6.0,						    -T_sq*tao_bias/2 						  ,T*tao_bias         ;

    Q_lead <<  q_l,  0,  0,
    			0, q_l, 0,
    			0,  0,  q_l ;
    Q_neigh << q_n,  0,  0,
    			0, q_n, 0,
    			0,  0,  q_n ;

    Q.block<3,3>(0,0) = block_Q;
    Q.block<3,3>(3,3) = block_Q;
    Q.block<3,3>(6,6) = block_Q * m_z_damping_factor;
    Q *= m_Q_scale;
    Q.block<3,3>(9,9) = Q_lead;
    Q.block<3,3>(12,12) = Q_neigh;
    //ROS_WARN("covariance:## %f, ## %f, ## %f",(T_cub*tao_acc/3.0)+(T_cub*T_sq)*tao_bias/20.0 , T*tao_acc+(T_cub*tao_bias/3), T*tao_bias);

    // if(count%250 == 0)
    // std::cout << P << '\n'<<'\n';
    // M is the predicted covariance matrix
	M = F*P*F.transpose() + Q;

    // time update
    m_last_range_time = msg->header.stamp.toSec();
    //count++;
    error = X.squaredNorm()-X_e.squaredNorm();
    if(error < error_threshold){
        //ROS_WARN("\n sucess too large: %f", error);
        setState(X_e);
        setCovariance(M);
        return ;
    } else {

        ROS_WARN("\n Estimate too large: %f", error);
        return ;
    }

}

void correction_step_leader(const dwm1001::anchor::ConstPtr& msg)
{
	r_pred_l = std::sqrt( std::pow(X(0) -  X(9), 2) +
                          std::pow(X(3) - X(10), 2) +
                          std::pow(X(6) - X(11), 2) ) + 1e-5;

	r_meas_l = msg->range;
    // H is the linearized measurement matrix
    H_l << (X(0) -  X(9))/r_pred_l, 0, 0,
           (X(3) - X(10))/r_pred_l, 0, 0,
           (X(6) - X(11))/r_pred_l, 0, 0,
           (X(9) - X(0))/r_pred_l, (X(10) - X(3))/r_pred_l, (X(11) - X(6))/r_pred_l,
         	0,	0,	0;

    // K is the Kalman Gain
    sigma_r_l = double(precisionRangeErrEst_l) / 1000.0;
    R_l = std::pow(sigma_r_l,2) * m_R_scale_l;
    //ROS_WARN("R_matrix:## %f",R);
    K_l = P*H_l.transpose() / ( (H_l*P*H_l.transpose())(0,0) + R_l );
    // Update P for the a posteriori covariance matrix
    P_p_l = ( Eigen::MatrixXd::Identity(15,15) - K_l*H_l ) * P;
    // Return the measurement innovation
    error_l = std::fabs(r_meas_l - r_pred_l);
    // Update the state
    X_p_l = X + K_l * (r_meas_l - r_pred_l);
    // decide to take the range info or not.
    if(error_l < error_threshold){
        //ROS_WARN("\n sucess too large: %f", error);
        setState(X_p_l);
        setCovariance(P_p_l);

        return ;
    } else {

        ROS_WARN("Anchor id , Update too large: %f --", error_l);
        std::cout << msg->device_id << "\n";
        return ;
    }

}

void correction_step_neigh(const dwm1001::anchor::ConstPtr& msg)
{
	r_pred_n = std::sqrt( std::pow(X(0) - X(12), 2) +
                          std::pow(X(3) - X(13), 2) +
                          std::pow(X(6) - X(14), 2) ) + 1e-5;

	r_meas_n = msg->range;
    // H is the linearized measurement matrix
    H_n << (X(0) - X(12))/r_pred_n, 0, 0,
           (X(3) - X(13))/r_pred_n, 0, 0,
           (X(6) - X(14))/r_pred_n, 0, 0,
         	0,	0,	0,
           (X(12) - X(0))/r_pred_n, (X(13) - X(3))/r_pred_n, (X(14) - X(6))/r_pred_n ;

    // K is the Kalman Gain
    sigma_r_n = double(precisionRangeErrEst_n) / 1000.0;
    R_n = std::pow(sigma_r_n,2) * m_R_scale_n;
    //ROS_WARN("R_matrix:## %f",R);
    K_n = P*H_n.transpose() / ( (H_n*P*H_n.transpose())(0,0) + R_n );
    // Update P for the a posteriori covariance matrix
    P_p_n = ( Eigen::MatrixXd::Identity(15,15) - K_n*H_n ) * P;
    // Return the measurement innovation
    error_n = std::fabs(r_meas_n - r_pred_n);
    // Update the state
    X_p_n = X + K_n * (r_meas_n - r_pred_n);
    // decide to take the range info or not.
    if(error < error_threshold){
        //ROS_WARN("\n sucess too large: %f", error);
        setState(X_p_n);
        setCovariance(P_p_n);

        return ;
    } else {

        ROS_WARN("Anchor id , Update too large: %f --", error_n);
        std::cout << msg->device_id << "\n";
        return ;
    }

}

// void anchor_lead_cb(const dwm1001::anchor::ConstPtr& msg)
// {
// 	correction_step_leader(msg);
// }

// void anchor_neigh_cb(const dwm1001::anchor::ConstPtr& msg)
// {
// 	correction_step_neigh(msg);
// }

void position_lead_cb(const geometry_msgs::PoseStamped::ConstPtr& msg)
{
	x_l =  msg->pose.position.x;
    y_l =  msg->pose.position.y;
    z_l =  msg->pose.position.z;
}

void position_neigh_cb(const geometry_msgs::PoseStamped::ConstPtr& msg)
{
	x_n =  msg->pose.position.x;
    y_n =  msg->pose.position.y;
    z_n =  msg->pose.position.z;
}

void param(ros::NodeHandle& nh)
{
    nh.getParam("KalmanFilter_swarm/start_x", x);
    nh.getParam("KalmanFilter_swarm/start_y", y);
    nh.getParam("KalmanFilter_swarm/start_z", z);
    nh.getParam("KalmanFilter_swarm/lead_x", x_l);
    nh.getParam("KalmanFilter_swarm/lead_y", y_l);
    nh.getParam("KalmanFilter_swarm/lead_z", z_l);
    nh.getParam("KalmanFilter_swarm/neighbor_x", x_n);
    nh.getParam("KalmanFilter_swarm/neighbor_y", y_n);
    nh.getParam("KalmanFilter_swarm/neighbor_z", z_n);
    nh.getParam("KalmanFilter_swarm/error_threshold", error_threshold);
    nh.getParam("KalmanFilter_swarm/m_R_scale_leader", m_R_scale_l);
    nh.getParam("KalmanFilter_swarm/precisionRangeErrEst_leader", precisionRangeErrEst_l);
    nh.getParam("KalmanFilter_swarm/m_R_scale_neighbor", m_R_scale_n);
    nh.getParam("KalmanFilter_swarm/precisionRangeErrEst_neighbor", precisionRangeErrEst_n);
    nh.getParam("KalmanFilter_swarm/m_kalman_sigma_a", m_kalman_sigma_a);
    nh.getParam("KalmanFilter_swarm/precisionRangeMm", precisionRangeMm);
    nh.getParam("KalmanFilter_swarm/leader_covariance", q_l);
    nh.getParam("KalmanFilter_swarm/neighbor_covariance", q_n);
    nh.getParam("KalmanFilter_swarm/m_tao_acc_sqrt", m_tao_acc_sqrt);
    nh.getParam("KalmanFilter_swarm/m_tao_bias_sqrt", m_tao_bias_sqrt );
    nh.getParam("KalmanFilter_swarm/m_z_damping_factor", m_z_damping_factor);
    nh.getParam("KalmanFilter_swarm/m_Q_scale", m_Q_scale);
}

void Initialize(ros::NodeHandle& nh)
{

    param(nh);
    // ROS_WARN("m_Q_scale %f", m_Q_scale);
    // ROS_WARN("x %f", x);

    m_kalman_sigma_a = 0.125;
    m_snr_threshold = -100;

    Eigen::MatrixXd nine_cov = Eigen::MatrixXd::Identity(15,15);
    nine_cov(0,0) = 0.001;
    nine_cov(3,3) = 0.001;
    nine_cov(6,6) = 0.001;
    // set cov of vel
    nine_cov(1,1) = 0.01;
    nine_cov(4,4) = 0.01;
    nine_cov(7,7) = 0.01;
    // set cov of acc_bias
    nine_cov(2,2) = 0.01;
    nine_cov(5,5) = 0.01;
    nine_cov(8,8) = 0.01;

    nine_cov(9,9)   = 0.001;
    nine_cov(10,10) = 0.001;
    nine_cov(11,11) = 0.001;

    nine_cov(12,12) = 0.001;
    nine_cov(13,13) = 0.001;
    nine_cov(14,14) = 0.001;

    setCovariance(nine_cov);
    X<<x,0,0,y,0,0,z,0,0,x_l,y_l,z_l,x_n,y_n,z_n;
    m_last_range_time = ros::Time::now().toSec();

}

int main(int argc, char** argv){

    ros::init(argc,argv,"KalmanFilter_swarm");
    ros::NodeHandle nh;
    //Initializinging the parameters
    Initialize(nh);

    //Subscriber and Publisher for the data. remapped in the launch file to the topic required
    ros::Subscriber Imu=nh.subscribe("imu",100,prediction_step);
    ros::Subscriber anchor_1 = nh.subscribe("anchor_lead", 100,correction_step_leader);
    ros::Subscriber anchor_2 = nh.subscribe("anchor_neigh",100,correction_step_neigh);
    ros::Subscriber anchor_3 = nh.subscribe("position_leader",100,position_lead_cb);
    ros::Subscriber anchor_4 = nh.subscribe("position_neighour",100,position_neigh_cb);

    //ros::Publisher fused = nh.advertise<std_msgs::Float32MultiArray>("Filtered_data", 100);
    ros::Publisher fused_pose = nh.advertise<geometry_msgs::Vector3Stamped>("Filtered_pose", 10);
    ros::Publisher fused_vel = nh.advertise<geometry_msgs::Vector3Stamped>("Filtered_velocity", 10);
    ros::Publisher fused_acc = nh.advertise<geometry_msgs::Vector3Stamped>("Filtered_acc", 10);

    ros::Rate loop_rate(50);
    while(ros::ok()){
        param(nh);
    //	output.data.clear();
	//	for (int i = 0; i < 15; i++)
	//		output.data.push_back(X(i));

        pose.header.stamp = ros::Time::now();
        pose.vector.x=X(0);
        pose.vector.y=X(3);
        pose.vector.z=X(6);

        vel.header.stamp = ros::Time::now();
        vel.vector.x=X(1);
        vel.vector.y=X(4);
        vel.vector.z=X(7);

        acc_.header.stamp = ros::Time::now();
        acc_.vector.x=ax;
        acc_.vector.y=ay;
        acc_.vector.z=az;
    //    fused.publish(output);
        fused_pose.publish(pose);
        fused_vel.publish(vel);
        fused_acc.publish(acc_);
        ros::spinOnce();
        loop_rate.sleep();
    }
    return 0;
}


/*
Copyright (C) 2012 Mathias Eitz and Ronald Richter.
All rights reserved.

This file is part of the imdb library and is made available under
the terms of the BSD license (see the LICENSE file).
*/

#ifndef KMEANS_HPP
#define KMEANS_HPP

#include <vector>
#include <algorithm>
#include <iostream>
#include <set>

#include <boost/random.hpp>
#include <boost/thread/thread.hpp>
#include <boost/thread/mutex.hpp>
#include <boost/thread/locks.hpp>

#include <QTime>

#include "../search/distance.hpp"
#include "kmeans_init.hpp"

/**
 * @brief Standard kmeans clustering
 */
template <class collection_t, class dist_fn>
class kmeans
{
    typedef boost::mutex               mutex_t;
    typedef boost::lock_guard<mutex_t> locker_t;

    typedef typename collection_t::value_type sample_t;

    public:

    /**
     * @brief Standard k-means clustering given a distance function
     * @param collection Datastructure containing the samples to be clustered, typically a vector<vector<float> >, where the 'inner'
     * vector<float> would be a single sample.
     * @param numclusters Number of clusters to use.
     * @param initalgorithm Algorithm used to estimate the inital cluster centers
     * @param distfn Distance function used for comparing two samples.
     */
    kmeans(const collection_t& collection, std::size_t numclusters, KmeansInitAlgorithm initalgorithm = KmeansInitRandom, const dist_fn& distfn = dist_fn())
     : _collection(collection), _distfn(distfn), _centers(numclusters), _clusters(collection.size())
    {
        // get initial centers
        std::vector<std::size_t> initindices;
        if (initalgorithm == KmeansInitPlusPlus)
        {
            kmeans_init_plusplus(initindices, collection, numclusters, distfn);
        }
        else
        {
            kmeans_init_random(initindices, collection, numclusters);
        }

        for (std::size_t i = 0; i < initindices.size(); i++) _centers[i] = collection[initindices[i]];
    }

    /**
     * @brief Perform k-means clustering on the dataset provided in the constructor
     *
     * Iterates until at least one of the following two criteria are met:
     * - maximum number of iterations reached
     * - the fraction of samples that changed clusters is below minchangesfraction
     * @param maxiteration Maximum number of iterations
     * @param minchangesfraction Fraction of changes, if less changes happen in a certain iteration, clustering is done.
     */
    void run(std::size_t maxiteration, double minchangesfraction)
    {
        using namespace boost;

        // main iteration
        std::size_t iteration = 0;
        for (;;)
        {
            if (maxiteration > 0 && iteration == maxiteration) break;

            QTime time;
            time.start();
            std::size_t changes = 0;

            // distribute items on clusters in parallel
            thread_group pool;
            std::size_t idx = 0;
            mutex_t mtx;
            for (std::size_t i = 0; i < thread::hardware_concurrency(); i++)
            {
                pool.create_thread(bind(&kmeans::distribute_samples, this, ref(idx), ref(changes), ref(mtx)));
            }
            pool.join_all();

            iteration++;

            std::cout << "changes: " << changes << " distribution time: " << time.elapsed() << std::endl;

            if (changes <= std::ceil(_collection.size() * minchangesfraction)) break;

            // compute new centers
            std::vector<std::size_t> clustersize(_centers.size(), 0);
            for (std::size_t i = 0; i < _collection.size(); i++)
            {
                std::size_t k = _clusters[i];

                // if it is the first assignment for that cluster, then zero the center
                if (clustersize[k] == 0) std::fill(_centers[k].begin(), _centers[k].end(), 0);

                add_operation(_centers[k], _collection[i]);
                clustersize[k]++;
            }

            // assign new centers
            std::vector<std::size_t> invalid, valid;
            for (std::size_t i = 0; i < _centers.size(); i++)
            {
                if (clustersize[i] > 0)
                {
                    div_operation(_centers[i], clustersize[i]);
                    valid.push_back(i);
                }
                else
                {
                    invalid.push_back(i);
                }
                //std::cout << "clustersize " << i << ": " << clustersize[i] << std::endl;
            }

            // fix invalid centers, i.e. those with no members
            while (!invalid.empty() && !valid.empty())
            {
                std::size_t current = invalid.back();
std::cout << "handle invalid clusters: " << invalid.size() << std::endl;
                // compute for each valid cluster the variance
                // of distances to all members and get the
                // most distant member
                std::vector<double> maxdist(_centers.size(), 0.0);
                std::vector<double> variance(_centers.size(), 0.0);
                std::vector<std::size_t> farthest(_centers.size());
                for (std::size_t i = 0; i < valid.size(); i++)
                {
                    std::size_t c = valid[i];

                    for (std::size_t k = 0; k < _collection.size(); k++)
                    {
                        if (_clusters[k] != c) continue;

                        double d = _distfn(_collection[k], _centers[c]);
                        if (d > maxdist[c])
                        {
                            maxdist[c] = d;
                            farthest[c] = k;
                        }
                        variance[c] += d*d;
                    }
                    variance[c] /= clustersize[c];
                }

                // get cluster with highest variance and make
                // the farthest member of that cluster the
                // new center
                std::size_t c = std::distance(variance.begin(), std::max_element(variance.begin(), variance.end()));
                _centers[current] = _collection[farthest[c]];
                _clusters[farthest[c]] = current;

std::cout << "reassign " << current << " to sample " << farthest[c] << " of cluster " << c << std::endl;

                valid.pop_back();
                invalid.pop_back();
            }

std::cout << "iteration " << iteration << " time: " << time.elapsed() << std::endl;
        }

std::cout << "kmeans iterations: " << iteration << std::endl;
    }

    /// Run clustering, using theoretically unlimited number of iterations. Clustering will
    /// stop when the fraction of changes falls below 0.01
    void run_default()
    {
        this->run(std::numeric_limits<std::size_t>::max(), 0.01);
    }

    /// Vector of cluster membership: clusters[i] = j means that the sample with index i
    /// is a member of cluster j
    const std::vector<std::size_t>& clusters() const
    {
        return _clusters;
    }

    /// Vector of cluster centers
    const std::vector<sample_t>& centers() const
    {
        return _centers;
    }

    /// Convenience function generating a clustering table:
    /// table[i][j] = k means that the sample with index k belongs to cluster i.
    template <class index_t>
    void make_cluster_table(std::vector<std::vector<index_t> >& table)
    {
        table.resize(_centers.size());
        for (std::size_t i = 0; i < _clusters.size(); i++) table[_clusters[i]].push_back(i);
    }

    template <class T>
    static void add_operation(T& lhs, const T& rhs)
    {
        for (std::size_t i = 0; i < lhs.size(); i++) lhs[i] += rhs[i];
    }

    template <class T>
    static void div_operation(T& lhs, double rhs)
    {
        for (std::size_t i = 0; i < lhs.size(); i++) lhs[i] /= rhs;
    }

    private:

    void distribute_samples(std::size_t& index, std::size_t& changes, mutex_t& mutex)
    {
        std::vector<double> dists(_centers.size());
        std::size_t currentchanges = 0;

        for (;;)
        {
            std::size_t i;

            {
                locker_t locker(mutex);
                if (index == _collection.size()) break;
                i = index++;
            }

            // compute distance of current point to every center
            std::transform(_centers.begin(), _centers.end(), dists.begin(), boost::bind(_distfn, boost::ref(_collection[i]), boost::arg<1>()));

            // find the minimum distance, i.e. the nearest center
            std::size_t c = std::distance(dists.begin(), std::min_element(dists.begin(), dists.end()));

            // update cluster membership
            {
                locker_t locker(_mutex);

                if (_clusters[i] != c)
                {
                    _clusters[i] = c;
                    currentchanges++;
                }
//if (i % 1000 == 0) std::cout << "kmeans distribute: " << i << std::endl;
            }
        }

        {
            locker_t locker(mutex);
            changes += currentchanges;
        }
    }

    const collection_t& _collection;
    const dist_fn&      _distfn;

    std::vector<sample_t>    _centers;
    std::vector<std::size_t> _clusters;

    boost::mutex        _mutex;
};

#endif // KMEANS_HPP


/***************************************************************
 * Author:    Yixin Zhuang (yixin.zhuang@gmail.com)
 **************************************************************/

#include "core/Segmentation.h"

#include <fstream>
#include <queue>

//i use boost_1_67_0
#include <boost/config.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>
using namespace boost;
//boost graph
typedef adjacency_list < vecS, vecS, undirectedS,
	no_property, property < edge_weight_t, double > > graph_t;
typedef graph_traits < graph_t >::vertex_descriptor vertex_descriptor;
typedef graph_traits < graph_t >::edge_descriptor edge_descriptor;
typedef std::pair<int, int> Edge;

using namespace GeoProperty;

#include "andres/graph/graph.hxx"
#include "andres/graph/complete-graph.hxx"
#include "andres/graph/multicut-lifted/greedy-additive.hxx"
#include "andres/graph/multicut-lifted/kernighan-lin.hxx"

void Mitani_WaterShed::distanceToFeature_ShortestPath(std::list<MyMesh::Vertex>& vers, std::list<MyMesh::Edge>& edges, std::set<int>& sources, std::vector<double>& res)
{
	//build dual graph;

	int superNode = vers.size();
	int vNum = vers.size() + 1; //will add a super node
	int eNum = edges.size() + sources.size();//will add edges connecting super node and sources

	// init graph
	graph_t g(vNum);
	property_map<graph_t, edge_weight_t>::type weightmap = get(edge_weight, g);
	for (auto i = edges.begin(); i != edges.end(); i++)
	{
		int nodeID[] = { i->vertex_iter(0)->id(), i->vertex_iter(1)->id() };
		graph_traits < graph_t >::edge_descriptor e;
		bool inserted;
		boost::tie(e, inserted) = add_edge(nodeID[0], nodeID[1], g);
		if (!inserted) cout << "insert edge failed" << endl;
		weightmap[e] = i->length();
	}
	for (auto i = sources.begin(); i != sources.end(); i++)
	{
		graph_traits < graph_t >::edge_descriptor e;
		bool inserted;
		boost::tie(e, inserted) = add_edge(*i, superNode, g);
		if (!inserted) cout << "insert edge failed" << endl;
		weightmap[e] = 0;
	}

	//shortest path;
	std::vector<vertex_descriptor> p(vNum);
	std::vector<double> d(vNum);
	vertex_descriptor s = vertex(superNode, g);
	dijkstra_shortest_paths(g, s, predecessor_map(&p[0]).distance_map(&d[0]));

	d.pop_back();
	double maxDist = *std::max_element(d.begin(), d.end());
	res.resize(d.size());
	for (int i = 0; i < d.size(); i++)
	{
		res[i] = maxDist - d[i];
	}
}
void Mitani_WaterShed::distanceToFeature_ShortestPath(MGraph& pg, std::set<int>& sources, std::vector<double>& res)
{
	//build dual graph;

	int superNode = pg.vNum;
	int vNum = pg.vNum + 1; //will add a super node
	int eNum = pg.eNum + sources.size();//will add edges connecting super node and sources

	// init graph
	graph_t g(vNum);
	property_map<graph_t, edge_weight_t>::type weightmap = get(edge_weight, g);
	for (auto i = pg.es.begin(); i != pg.es.end(); i++)
	{
		int nodeID[] = { i->n1, i->n2 };
		graph_traits < graph_t >::edge_descriptor e;
		bool inserted;
		boost::tie(e, inserted) = add_edge(nodeID[0], nodeID[1], g);
		if (!inserted) cout << "insert edge failed" << endl;
		weightmap[e] = i->w;
	}
	for (auto i = sources.begin(); i != sources.end(); i++)
	{
		graph_traits < graph_t >::edge_descriptor e;
		bool inserted;
		boost::tie(e, inserted) = add_edge(*i, superNode, g);
		if (!inserted) cout << "insert edge failed" << endl;
		weightmap[e] = 0;
	}

	//shortest path;
	std::vector<vertex_descriptor> p(vNum);
	std::vector<double> d(vNum);
	vertex_descriptor s = vertex(superNode, g);
	dijkstra_shortest_paths(g, s, predecessor_map(&p[0]).distance_map(&d[0]));

	d.pop_back();
	double maxDist = *std::max_element(d.begin(), d.end());
	res.resize(d.size());
	for (int i = 0; i < d.size(); i++)
	{
		res[i] = maxDist - d[i];
	}
}

int Mitani_WaterShed::grow(const std::vector<std::list<MyMesh::Vertex>::iterator>& vers, const std::vector<double>& distanceToFeature, std::vector<bool>& isExtrema, std::vector<unsigned>& vertexLabels)
{
	//init indexed items, and the flood fronts;
	int vNum = distanceToFeature.size();
	std::vector<bool> unIndexed(vNum, true);

	vertexLabels.clear(); vertexLabels.resize(vNum, UINT_MAX);
	int labelId = 0;
	std::priority_queue<HighFunction> floodFront;
	for (int i = 0; i < vNum; i++)
	{
		if (isExtrema[i])
		{
			unIndexed[i] = false;
			vertexLabels[i] = labelId++;
			floodFront.push(HighFunction(i, distanceToFeature[i]));
		}
	}

	const auto vts = vers;
	while (!floodFront.empty())
	{
		HighFunction v = floodFront.top(); floodFront.pop();

		for (int i = 0; i < vts[v.m_id]->vertex_iter().size(); i++)
		{
			int id = vts[v.m_id]->vertex_iter()[i]->id();
			if (unIndexed[id]) //unindexed neighbore;
			{
				HighFunction newV(id, distanceToFeature[id]);
				floodFront.push(newV);

				unIndexed[id] = false;
				vertexLabels[id] = vertexLabels[v.m_id];
			}
		}
	}

	cout << "cluster number:" << labelId << endl;
	return labelId;
}
//////////////////////////////////////////////////////////////////////////
//grow2 -- Implement of Paper: Analysis and Comparison of Algorithms for Morse Decompositions on Triangulated Terrains
//by Maria Vitali, Leila De Floriani, Paola Magillo
//detailed in section 5:watershed algorithms, page 9.
//the output contains not just the cluster labels of vertices, but also the boundary vertices(between basins).
int Mitani_WaterShed::grow2(MGraph& pg, const std::vector<double>& distanceToFeature, const std::vector<std::set<unsigned> >& neighbours, std::vector<unsigned>& vertexLabels)
{
	//init indexed items, and the flood fronts;
	int vNum = distanceToFeature.size();
	std::vector<bool> unIndexed(vNum, true);
	std::priority_queue<HighFunction> floodFront;
	for (int i = 0; i < vNum; i++)
	{
		floodFront.push(HighFunction(i, distanceToFeature[i]));
	}

	int labelId = 0;
	std::vector<int> labels(vNum, -1);
	while (!floodFront.empty())
	{
		HighFunction v = floodFront.top(); floodFront.pop();
		std::set<int> ls;
		for (auto n = neighbours[v.m_id].begin(); n != neighbours[v.m_id].end(); n++)
		{
			if (!unIndexed[*n] && labels[*n] != -2) //neighbours assigned as basin;
			{
				ls.insert(labels[*n]);
			}
		}

		if (ls.empty())
		{
			labels[v.m_id] = labelId; labelId++;//new cluster begins...
		}
		else if (ls.size() == 1)
		{
			labels[v.m_id] = *ls.begin();//basin node
		}
		else
		{
			labels[v.m_id] = -2;//watershed node
		}

		unIndexed[v.m_id] = false;
	}

	vertexLabels.resize(vNum, 0);
	for (int i = 0; i < vNum; i++)
	{
		if (labels[i] == -2)
		{
			vertexLabels[i] = UINT_MAX; //watershed node
		}
	}

	return labelId;
}
void Mitani_WaterShed::thresholdClusters(MyMesh* mesh, std::vector<unsigned>& faceLabels, unsigned& segNum, std::vector<bool>& isFeature, double thres)
{
	// In our case, we use area as measurement of cluster size, for merging. Other criteria, like perimeter, would only require few changes of the code.
	double averageArea = 0;
	std::vector<double> faceAreas(mesh->getFaces().size());
	for (auto i = mesh->getFaces().begin(); i != mesh->getFaces().end(); i++)
	{
		faceAreas[i->id()] = i->triangleCost(2);
		averageArea += faceAreas[i->id()];
	}

	thres = averageArea * thres*0.01; //thres percent (thres%) of total area;

	//initialize area of all clusters;
	std::vector<double> scores(segNum, 0.0);
	for (auto i = mesh->getFaces().begin(); i != mesh->getFaces().end(); i++)
	{
		scores[faceLabels[i->id()]] += faceAreas[i->id()];
	}

	//shared boundary length between clusters;
	std::vector<double> sharesRow(segNum, 0.0);
	std::vector<std::vector<double> > sharesMatrix(segNum, sharesRow);
	for (auto i = mesh->getEdges().begin(); i != mesh->getEdges().end(); i++)
	{
		if (i->manifold())
		{
			int l1 = faceLabels[i->face_iter(0)->id()];
			int l2 = faceLabels[i->face_iter(1)->id()];

			if (l1 != l2 && !isFeature[i->face_iter(0)->id()] && !isFeature[i->face_iter(1)->id()])
			{
				sharesMatrix[l1][l2] = sharesMatrix[l2][l1] += i->length(); //in dual graph case, use i->dualLength;
			}
		}
	}

	//build adjacency and priority queue;
	std::multiset<ClusterArea> clusterQueue;
	for (int i = 0; i < scores.size(); i++)
	{
		clusterQueue.insert(ClusterArea(i, scores[i]));
	}
	if (clusterQueue.size() != scores.size())
	{
		cout << "numerical issue" << endl;
	}
	//adjcents between clusters;
	std::list<std::multiset<DecreaseOrderExt> > clusterAdjcencyList(segNum);
	std::vector<std::list<std::multiset<DecreaseOrderExt> >::iterator> clusterAdjcency;
	for (auto i = clusterAdjcencyList.begin(); i != clusterAdjcencyList.end(); i++)
	{
		clusterAdjcency.push_back(i);
	}

	for (auto i = mesh->getEdges().begin(); i != mesh->getEdges().end(); i++)
	{
		if (i->manifold())
		{
			int l1 = faceLabels[i->face_iter(0)->id()];
			int l2 = faceLabels[i->face_iter(1)->id()];

			if (l1 != l2)
			{
				if (clusterAdjcency[l1]->find(DecreaseOrderExt(l2, scores[l2], sharesMatrix[l1][l2])) == clusterAdjcency[l1]->end())
					clusterAdjcency[l1]->insert(DecreaseOrderExt(l2, scores[l2], sharesMatrix[l1][l2]));

				if (clusterAdjcency[l2]->find(DecreaseOrderExt(l1, scores[l1], sharesMatrix[l1][l2])) == clusterAdjcency[l2]->end())
					clusterAdjcency[l2]->insert(DecreaseOrderExt(l1, scores[l1], sharesMatrix[l1][l2]));
			}
		}
	}

	//clusters
	std::list<std::set<int>> clusters(segNum);
	std::vector<std::list<std::set<int>>::iterator> iclusters;
	for (auto i = clusters.begin(); i != clusters.end(); i++)
	{
		iclusters.push_back(i);
	}
	for (int i = 0; i < faceLabels.size(); i++)
	{
		iclusters[faceLabels[i]]->insert(i);
	}

// 	cout << "thres:" << thres << endl;
	while (!clusterQueue.empty())
	{
		ClusterArea ci = *clusterQueue.begin(); clusterQueue.erase(clusterQueue.begin());
		if (ci.m_val > thres)
		{
			break;
		}
		if (clusterAdjcency[ci.m_id]->empty())
		{
			continue;
		}

		DecreaseOrderExt cj_ = *clusterAdjcency[ci.m_id]->begin();
		ClusterArea cj(cj_.m_id, cj_.m_val);

		clusterAdjcency[ci.m_id]->erase(clusterAdjcency[ci.m_id]->begin()); //remove cj from ci's

		auto tp = clusterAdjcency[cj.m_id]->begin();
		for (; tp != clusterAdjcency[cj.m_id]->end(); tp++)
		{
			if (tp->m_id == ci.m_id )
				break;
		}
		if (tp == clusterAdjcency[cj.m_id]->end())
		{
			cout << "invalid merge" << endl;
		}
		else
		{
			clusterAdjcency[cj.m_id]->erase(tp);
		}

		auto i = std::find(clusterQueue.begin(), clusterQueue.end(), cj);
		if (i == clusterQueue.end())
		{
			cout << "invalid reference" << endl;
		}
		else
		{
			clusterQueue.erase(i);
		}

		//0. merge ci to cj.
		//1. cj is still in clusterQueue, need to update it, the new area is not just sum of ci and cj.
		//2. clear ci's clusterAdjacency, and add adj to cj, if not existed in cj. add cj to ci's adjacency, if not existed.

		//0. merge ci to cj.
		for (auto i = iclusters[ci.m_id]->begin(); i != iclusters[ci.m_id]->end(); i++)
		{
			faceLabels[*i] = cj.m_id;
		}
		iclusters[cj.m_id]->insert(iclusters[ci.m_id]->begin(), iclusters[ci.m_id]->end());
		iclusters[ci.m_id]->clear();

		//1. cj is still in clusterQueue, need to update it, the new area is  just sum of ci and cj.
		//2. clear ci's clusterAdjacency, and add it's adjacency to cj, if not existed in cj, and cj to it's adjacency, if not existed in...
		// ci's = {ci_a, ci_...}
		double newScore = ci.m_val + cj.m_val;
		clusterQueue.insert(ClusterArea(cj.m_id,newScore));

		for (auto i = clusterAdjcency[ci.m_id]->begin(); i != clusterAdjcency[ci.m_id]->end(); i++)
		{
			int ci_a = i->m_id; // for each cluster

			//if ci_a not in cj, then add to cj, if exist, then update their shared boundary, m_val_ext;
			//if cj not in ci_a, then add to ci_a, if exist, then update their shared boundary, m_val_ext;
			auto ip = clusterAdjcency[ci_a]->begin();
			for (; ip != clusterAdjcency[ci_a]->end(); ip++)
			{
				if (ip->m_id == cj.m_id && ip->m_val == cj.m_val)
					break;
			}
			if (ip == clusterAdjcency[ci_a]->end())
			{
				clusterAdjcency[ci_a]->insert(DecreaseOrderExt(cj.m_id, newScore, i->m_val_ext));
			}
			else
			{
				double temp = ip->m_val_ext;
				clusterAdjcency[ci_a]->erase(ip);
				clusterAdjcency[ci_a]->insert(DecreaseOrderExt(cj.m_id, newScore, i->m_val_ext + temp));
			}

			ip = clusterAdjcency[cj.m_id]->begin();
			for (; ip != clusterAdjcency[cj.m_id]->end(); ip++)
			{
				if (ip->m_id == i->m_id && ip->m_val == i->m_val)
					break;
			}
			if (ip == clusterAdjcency[cj.m_id]->end())
			{
				clusterAdjcency[cj.m_id]->insert(DecreaseOrderExt(i->m_id, i->m_val, i->m_val_ext));
			}
			else
			{
				double temp = ip->m_val_ext;
				clusterAdjcency[cj.m_id]->erase(ip);
				clusterAdjcency[cj.m_id]->insert(DecreaseOrderExt(i->m_id, i->m_val, i->m_val_ext + temp));
			}

			//remove ci from ci_a
			ip = clusterAdjcency[ci_a]->begin();
			for (; ip != clusterAdjcency[ci_a]->end(); ip++)
			{
				if (ip->m_id == ci.m_id && ip->m_val == ci.m_val)
					break;
			}
			if (ip == clusterAdjcency[ci_a]->end())
			{
				cout << endl << ci_a << " error " << ci.m_id << " " << ci.m_val << endl;
			}
			else
			{
				clusterAdjcency[ci_a]->erase(ip);
			}
		}

		clusterAdjcency[ci.m_id]->clear();

		//for all cj_a in cj, even though not appeared in ci, need to update cj_a's element cj...
		for (auto i = clusterAdjcency[cj.m_id]->begin(); i != clusterAdjcency[cj.m_id]->end();i++)
		{
			auto ip = clusterAdjcency[i->m_id]->begin();
			for (; ip != clusterAdjcency[i->m_id]->end(); ip++)
			{
				if (ip->m_id == cj.m_id)
					break;
			}
			if (ip == clusterAdjcency[i->m_id]->end())
			{
				cout << "bad reference" << endl;
			}
			else
			{
				clusterAdjcency[i->m_id]->erase(ip);
				clusterAdjcency[i->m_id]->insert(DecreaseOrderExt(cj.m_id, newScore,i->m_val_ext));
			}
		}

	}

	//update label;
	int ind = 0;
	for (auto i = clusters.begin(); i != clusters.end(); i++)
	{
		if (i->empty())
			continue;

		for (auto f = i->begin(); f != i->end(); f++)
		{
			faceLabels[*f] = ind;
		}

		ind++;
	}
	segNum = ind;
}

void MeshSegment::convertFaceToVertexLabelling(std::vector<unsigned>& vertexLabel,std::vector<unsigned>& faceLabel)
{
	unsigned vNum = myMesh->getVertices().size();
	std::vector<std::vector<unsigned> > vertexMultiLabels(vNum);
	for (auto i = myMesh->getFaces().begin(); i != myMesh->getFaces().end(); i++)
	{
		for (unsigned j = 0; j < 3; j++)
		{
			vertexMultiLabels[i->vertex_iter(j)->id()].push_back(faceLabel[i->id()]);
		}
	}

	vertexLabel.clear(); vertexLabel.resize(vNum);
	for (int i = 0; i < vNum; i++)
	{
		std::map<unsigned, unsigned> st;
		std::map<unsigned, unsigned>::iterator it;
		for (int j = 0; j < vertexMultiLabels[i].size(); j++)
		{
			it = st.find(vertexMultiLabels[i][j]);
			if (it == st.end())
			{
				st[vertexMultiLabels[i][j]] = 1;
			}
			else
			{
				st[vertexMultiLabels[i][j]] = it->second + 1;
			}
		}

		std::pair<unsigned, unsigned> majorPair(vertexMultiLabels[i][0],1);
		for (it = st.begin(); it != st.end(); it++)
		{
			if (majorPair.second < it->second)
			{
				majorPair.first = it->first;
				majorPair.second = it->second;
			}
		}
		vertexLabel[i] = majorPair.first;
	}

	std::vector<double> weights;
	getWeights(edgeWeightParameter,weights);

	double cutCost = 0;
	for (auto e = myMesh->getEdges().begin(); e != myMesh->getEdges().end(); e++)
	{
		if (vertexLabel[e->vertex_iter(0)->id()] != vertexLabel[e->vertex_iter(1)->id()])
		{
			cutCost += weights[e->id()];
			graphFeature.ef[e->id()].isCut = true;
		}
		else
		{
			graphFeature.ef[e->id()].isCut = false;
		}
	}
	cout << "Cost:" << cutCost << endl;

	unsigned eNum = myMesh->getEdges().size();
	vertexLabel.clear(); vertexLabel.resize(vNum);
	segNumber = 0;
	std::vector<bool> visitedVer(vNum, false);
	std::vector<bool> visitedEdge(eNum, false);
	for (int i = 0; i < vNum; i++)
	{
		if (visitedVer[i] == true) continue;

		std::vector<int> frontVer(1, i);
		//propagates vertex with same label, and group them into a subgraph
		while (!frontVer.empty())
		{
			int fv = frontVer.back(); frontVer.pop_back();
			vertexLabel[fv] = segNumber;

			auto v = mg.vs[fv].vadjs.begin();
			auto e = mg.vs[fv].eadjs.begin();
			for (; v != mg.vs[fv].vadjs.end(); v++, e++)
			{
				if (graphFeature.ef[*e].isCut || visitedEdge[*e] == true)
					continue;

				visitedEdge[*e] = true;

				if (visitedVer[*v] == true)
					continue;

				visitedVer[*v] = true;

				frontVer.push_back(*v);
			}
		}
		segNumber++;
	}

	cout << endl << "cluster number :" << segNumber << endl;
}
void MeshSegment::convertVertexToFaceLabelling(std::vector<unsigned>& vertexLabel,std::vector<unsigned>& faceLabel)
{
	unsigned fNum = myMesh->getFaces().size();
	std::vector<std::vector<unsigned> > faceMultiLabels(fNum);
	for (auto i = myMesh->getFaces().begin(); i != myMesh->getFaces().end(); i++)
	{
		for (unsigned j = 0; j < 3; j++)
		{
			faceMultiLabels[i->id()].push_back(vertexLabel[i->vertex_iter(j)->id()]);
		}
	}

	faceLabel.clear();	faceLabel.resize(fNum);
	for (int i = 0; i < fNum; i++)
	{
		std::map<unsigned, unsigned> st;
		std::map<unsigned, unsigned>::iterator it;
		for (int j = 0; j < faceMultiLabels[i].size(); j++)
		{
			it = st.find(faceMultiLabels[i][j]);
			if (it == st.end())
			{
				st[faceMultiLabels[i][j]] = 1;
			}
			else
			{
				st[faceMultiLabels[i][j]] = it->second + 1;
			}
		}

		std::pair<unsigned, unsigned> majorPair(faceMultiLabels[i][0], 1);
		for (it = st.begin(); it != st.end(); it++)
		{
			if (majorPair.second < it->second)
			{
				majorPair.first = it->first;
				majorPair.second = it->second;
			}
		}

		faceLabel[i] = majorPair.first;
	}
}

///////////////////OVER SEGMENTATION///////////////////////////////
void MeshSegment::Mitani_Watershed()
{
	clock_t tstr = clock();

	//find sources;
	std::set<int> sources;
	for (auto e_it = myMesh->getEdges().begin(); e_it != myMesh->getEdges().end(); e_it++)
	{
		if (graphFeature.ef[e_it->id()].lab != -1)
		{
			sources.insert(e_it->vertex_iter(0)->id());
			sources.insert(e_it->vertex_iter(1)->id());
		}
	}

	ws.distanceToFeature_ShortestPath(myMesh->getVertices(), myMesh->getEdges(), sources, ws.distanceToFeature);

	//local extrema, and growing
	auto& vers = myMesh->getVIter();
	std::vector<bool> isExtrema(vers.size(), true);
	int ringSize = mitani_Watershed_Ringsize;
	for (int i = 0; i < vers.size(); i++)
	{
		int v1 = i;
		if (isExtrema[v1])
		{
			int times = ringSize;
			std::set<int> fronts; fronts.insert(v1);
			std::set<int> visited = fronts;
			std::set<int> nextFronts = fronts;
			while (times > 0)
			{
				for (auto j = fronts.begin(); j != fronts.end(); j++)
				{
					auto vj = vers[*j];
					for (int k = 0; k < vj->vertex_iter().size(); k++)
					{
						int v2 = vj->vertex_iter()[k]->id();
						if (visited.find(v2)!=visited.end())continue;
						nextFronts.insert(v2);
						visited.insert(v2);
					}
				}
				fronts = nextFronts;
				times--;
			}

			visited.erase(v1);
			for (auto j = visited.begin(); j != visited.end(); j++)
			{
				int v2 = *j;
				ws.distanceToFeature[v1] < ws.distanceToFeature[v2] ? isExtrema[v2] = false : isExtrema[v1] = false;
			}
		}
	}

	auto& multiCutLabels = vertexLabel;
	auto& multiCutNum = segNumber;
	multiCutNum = ws.grow(myMesh->getVIter(), ws.distanceToFeature, isExtrema, multiCutLabels);

	clock_t totalTime = clock() - tstr;
	cout << "Watershed Segmentation Use " << totalTime / 1000 << "sec" << totalTime % 1000 << "mm" << endl;

	std::vector<double> weights;
	getWeights(edgeWeightParameter, weights);

	vertexLabelBeforeMerge = vertexLabel;

	double cutCost = 0;
	for (auto e = myMesh->getEdges().begin(); e != myMesh->getEdges().end(); e++)
	{
		if (multiCutLabels[e->vertex_iter(0)->id()] != multiCutLabels[e->vertex_iter(1)->id()])
		{
			cutCost += weights[e->id()];
			graphFeature.ef[e->id()].isCut = true;
		}
		else
		{
			graphFeature.ef[e->id()].isCut = false;
		}
	}
	cout << "Cost:" << cutCost << endl;
}
void MeshSegment::Mitani_Watershed_Dual()
{
	clock_t tstr = clock();

	//build dual graph;
	std::list<MyMesh::Vertex> versArray(myMesh->getFaces().size());
	std::vector<std::list<MyMesh::Vertex>::iterator> vers(myMesh->getFaces().size());
	int ind = 0;
	for (auto v = versArray.begin(); v != versArray.end(); v++, ind++)
	{
		v->id() = ind;
		vers[ind] = v;
	}

	for (auto f = myMesh->getFaces().begin(); f != myMesh->getFaces().end(); f++)
	{
		vers[f->id()]->coordinate() = f->center_point();
	}

	std::vector<double> edgeCosts(myMesh->getEdges().size());
	std::vector<double> edgeLengths(myMesh->getEdges().size());
	{
		std::vector<Tensor>& tAnis = faceAnis;

		for (auto e_it = myMesh->getEdges().begin(); e_it != myMesh->getEdges().end(); e_it++)
		{
			if (e_it->manifold())
			{
				MyMesh::FaceIter f[] = { e_it->face_iter(0), e_it->face_iter(1) };
				unsigned fid[] = { f[0]->id(), f[1]->id() };
				Vec3 edgeVec = graphFeature.cf[f[0]->id()].rep - graphFeature.cf[f[1]->id()].rep;
				double edgeCost = 0;
				for (unsigned k = 0; k < 2; k++)
				{
					Vec2 edgeVec2D = Vec2(edgeVec.dot(tAnis[fid[k]].dir1), edgeVec.dot(tAnis[fid[k]].dir2));// two direction were swapped, since we computed dual edge's cost; and in faceAnis, mag1>mag2;
					edgeCost += sqrt(pow(edgeVec2D.x, 2)*tAnis[fid[k]].mag1 + pow(edgeVec2D.y, 2)*tAnis[fid[k]].mag2);
				}
				edgeCosts[e_it->id()] = edgeCost*0.5;
				edgeLengths[e_it->id()] = edgeVec.length();
			}
			else
			{
				MyMesh::FaceIter f[] = { e_it->face_iter(0) };
				unsigned fid[] = { f[0]->id(), f[1]->id() };
				Vec3 edgeVec = graphFeature.cf[f[0]->id()].rep - graphFeature.ef[e_it->id()].rep;
				double edgeCost = 0;
				for (unsigned k = 0; k < 1; k++)
				{
					Vec2 edgeVec2D = Vec2(edgeVec.dot(tAnis[fid[k]].dir1), edgeVec.dot(tAnis[fid[k]].dir2));// two direction were swapped, since we computed dual edge's cost; and in faceAnis, mag1>mag2;
					edgeCost += sqrt(pow(edgeVec2D.x, 2)*tAnis[fid[k]].mag1 + pow(edgeVec2D.y, 2)*tAnis[fid[k]].mag2);
				}
				edgeCosts[e_it->id()] = edgeCost + 1e-6; //make sure the boundary edge has weight larger than 0;
				edgeLengths[e_it->id()] = edgeVec.length();
			}
		}
	}

	std::list<MyMesh::Edge> edgesArray;
	for (auto e = myMesh->getEdges().begin(); e != myMesh->getEdges().end(); e++)
	{
		if (!e->manifold()) continue;

		int f1 = e->face_iter(0)->id();
		int f2 = e->face_iter(1)->id();

		vers[f1]->vertex_iter().push_back(vers[f2]);
		vers[f2]->vertex_iter().push_back(vers[f1]);

		MyMesh::Edge de;
		{//compute anis distance
			if (isAnisGeodesics_Watershed)
			{
				de.length() = edgeCosts[e->id()];
			}
			else
			{
				de.length() = edgeLengths[e->id()];
			}
		}
		de.vertex_iter(0) = vers[f1];
		de.vertex_iter(1) = vers[f2];
		edgesArray.push_back(de);
	}
	std::vector<std::list<MyMesh::Edge>::iterator> edges(edgesArray.size());
	ind = 0;
	for (auto e = edgesArray.begin(); e != edgesArray.end(); e++, ind++)
	{
		e->id() = ind;
		edges[ind] = e;
	}

	//find sources;
	std::set<int> sources;
	for (unsigned i = 0; i<crestEdgesVisible.size(); i++)
	{
		if (!crestEdgesVisible[i])
			continue;

		unsigned faceID = crestEdges[i][2];
		if (faceID>myMesh->getFaces().size())
		{
			continue; // crest line data is not alway clean
		}
		sources.insert(faceID);
	}
	for (auto f = userSketches.begin(); f != userSketches.end(); f++)
	{
		sources.insert(f->fid);
	}

	ws.distanceToFeature_ShortestPath(versArray, edgesArray, sources, ws.distanceToFeature);

	//local extrema, and growing
	std::vector<std::set<unsigned> > neighbours(myMesh->getFaces().size());
	for (auto f = myMesh->getFaces().begin(); f != myMesh->getFaces().end(); f++)
	{
		auto& fs = neighbours[f->id()];
		for (unsigned j = 0; j < 3; j++)
		{
			for (auto e = 0; e < f->vertex_iter(j)->edge_iter().size(); e++)
			{
				fs.insert(f->vertex_iter(j)->edge_iter()[e]->face_iter(0)->id());
				if (f->vertex_iter(j)->edge_iter()[e]->manifold())
					fs.insert(f->vertex_iter(j)->edge_iter()[e]->face_iter(1)->id());
			}
		}
	}

// 	std::vector<bool> isExtrema(neighbours.size(), true);
// 	for (unsigned i = 0; i < neighbours.size(); i++)
// 	{
// 		for (auto j = neighbours[i].begin(); j != neighbours[i].end(); j++)
// 		{
// 			if(ws.distanceToFeature[i] > ws.distanceToFeature[*j])
// 			{
// 				isExtrema[i] = false;
// 			}
// 		}
// 	}

	std::vector<unsigned> watershedLabels;
	ws.grow2(mg,ws.distanceToFeature, neighbours, watershedLabels);

	clock_t totalTime = clock() - tstr;
	cout << "Watershed Segmentation Use " << totalTime / 1000 << "sec" << totalTime % 1000 << "mm" << endl;

	std::vector<double> weights;
	getWeights(edgeWeightParameter, weights);

	double cutCost = 0;
	for (auto e = myMesh->getEdges().begin(); e != myMesh->getEdges().end(); e++)
	{
		if (!e->manifold()) continue;

		if (watershedLabels[e->face_iter(0)->id()] == UINT_MAX && watershedLabels[e->face_iter(1)->id()] == UINT_MAX)
		{
			cutCost += weights[e->id()];
			graphFeature.ef[e->id()].isCut = true;
		}
		else
		{
			graphFeature.ef[e->id()].isCut = false;
		}
	}
	cout << "Cost:" << cutCost << endl;

	unsigned vNum = myMesh->getVertices().size();
	unsigned eNum = myMesh->getEdges().size();
	vertexLabel.clear(); vertexLabel.resize(vNum);
	segNumber = 0;
	std::vector<bool> visitedVer(vNum, false);
	std::vector<bool> visitedEdge(eNum, false);
	for (int i = 0; i < vNum; i++)
	{
		if (visitedVer[i] == true) continue;

		std::vector<int> frontVer(1, i);
		//propagates vertex with same label, and group them into a subgraph
		while (!frontVer.empty())
		{
			int fv = frontVer.back(); frontVer.pop_back();
			vertexLabel[fv] = segNumber;

			auto v = mg.vs[fv].vadjs.begin();
			auto e = mg.vs[fv].eadjs.begin();
			for (; v != mg.vs[fv].vadjs.end(); v++, e++)
			{
				if (graphFeature.ef[*e].isCut || visitedEdge[*e] == true)
					continue;

				visitedEdge[*e] = true;

				if (visitedVer[*v] == true)
					continue;

				visitedVer[*v] = true;

				frontVer.push_back(*v);
			}
		}
		segNumber++;
	}

	cout << endl << "cluster number:" << segNumber << endl;

	vertexLabelBeforeMerge = vertexLabel;
}
////////////////////MERGING//////////////////////////////////
void MeshSegment::LMP_Merging(const std::vector<unsigned>& vertexLabel, std::vector< std::list< std::list< MGTriple > >::iterator >&pCIter,
	unsigned labelId, std::vector<unsigned>& newLabels)
{
	unsigned numE = 0;
	unsigned numV = labelId;
	std::vector<std::pair<std::pair<unsigned, unsigned>, double> > edges;
	for (int i = 0; i < pCIter.size(); i++){
		for (auto tpc = pCIter[i]->begin(); tpc != pCIter[i]->end(); tpc++)
		{
			if (i != tpc->j)
			{
				numE++;
				edges.push_back(std::pair<std::pair<unsigned, unsigned>, double>(std::pair<unsigned, unsigned>(i, tpc->j), tpc->val));
			}
		}
	}
	cout << "number of edges:" << numE << "  number of vertices:" << numV << endl;

	andres::graph::Graph<> graph;
	graph.insertVertices(numV);
	std::vector<double> weights(numE);
	for (int i = 0; i < numE; i++)
	{
		graph.insertEdge(edges[i].first.first, edges[i].first.second);
		weights[i] = edges[i].second;
	}

	std::vector<char> edge_labels(graph.numberOfEdges(), 1);
	andres::graph::multicut_lifted::greedyAdditiveEdgeContraction(graph, graph, weights, edge_labels);

	std::vector<char> out_labels(graph.numberOfEdges(), 1);
	andres::graph::multicut_lifted::kernighanLin(graph, graph, weights, edge_labels, out_labels);

	std::vector<unsigned> res(numV);
	std::vector<bool> visitedVer(numV, false);
	std::vector<bool> visitedEdge(numE, false);
	unsigned multiCutNum = 0;
	for (int i = 0; i < numV; i++)
	{
		if (visitedVer[i] == true) continue;

		std::vector<int> frontVer(1, i);
		//propagates vertex with same label, and group them into a subgraph
		while (!frontVer.empty())
		{
			int fv = frontVer.back(); frontVer.pop_back();
			res[fv] = multiCutNum;

			for (auto p = graph.adjacenciesToVertexBegin(fv); p != graph.adjacenciesToVertexEnd(fv); p++)
			{
				auto eid = p->edge();
				if (out_labels[eid] || visitedEdge[eid] == true)
					continue;

				visitedEdge[eid] = true;

				if (visitedVer[p->vertex()] == true)
					continue;

				visitedVer[p->vertex()] = true;

				frontVer.push_back(p->vertex());
			}
		}
		multiCutNum++;
	}
	//////////////////////////////////////////////////////////////////////////
	newLabels.clear(); newLabels.resize(vertexLabel.size());
	for (size_t i = 0; i < vertexLabel.size(); i++)
	{
		newLabels[i] = res[vertexLabel[i]];
	}
}
void MeshSegment::Mitani_Watershed_Dual_Partial(std::vector<unsigned>& fs)
{
	if (fs.empty())return;

	std::set<int> patchLabels;
	for (int i = 0; i < fs.size(); i++)
	{
		auto&f = myMesh->getFIter()[fs[i]];
		for (int j = 0; j < 3; j++)
		{
			patchLabels.insert(vertexLabel[f->vertex_iter(j)->id()]);
		}
	}

	std::vector<bool> faceInPatches(myMesh->getFaces().size(), true);
	subgraphVers.clear(); subgraphVers.resize(myMesh->getVertices().size(), true);
	unsigned numV = faceInPatches.size();
	for (auto f = myMesh->getFaces().begin(); f != myMesh->getFaces().end(); f++)
	{
		for (int j = 0; j < 3; j++)
		{
			if (std::find(patchLabels.begin(), patchLabels.end(), vertexLabel[f->vertex_iter(j)->id()]) == patchLabels.end())
			{
				faceInPatches[f->id()] = false;
				subgraphVers[f->vertex_iter(j)->id()] = false;
			}
		}
		if (faceInPatches[f->id()] == false) numV--;
	}

	MGraph pg;//partial graph;
	pg.vNum = numV;
	pg.vs.clear(); pg.vs.resize(pg.vNum);

	std::map<unsigned, unsigned> localIndToGlobalInd;
	std::map<unsigned, unsigned> globalIndToLocalInd;
	unsigned ind = 0;
	for (auto f = myMesh->getFaces().begin(); f != myMesh->getFaces().end(); f++)
	{
		if (faceInPatches[f->id()])
		{
			pg.vs[ind].id = ind;
			pg.vs[ind].coord = f->center_point();
			localIndToGlobalInd[ind] = f->id();
			globalIndToLocalInd[f->id()] = ind;
			ind++;
		}
	}

	pg.es.clear();
	ind = 0;
	for (auto e = myMesh->getEdges().begin(); e != myMesh->getEdges().end(); e++)
	{
		if (!e->manifold()) continue;

		int f1 = e->face_iter(0)->id();
		int f2 = e->face_iter(1)->id();
		if (!faceInPatches[f1] || !faceInPatches[f2]) continue;

		f1 = globalIndToLocalInd[f1];
		f2 = globalIndToLocalInd[f2];

		pg.vs[f1].vadjs.push_back(f2);
		pg.vs[f2].vadjs.push_back(f1);

		MGraph::Edge de;
		de.w = e->cost();
		de.n1 = f1;
		de.n2 = f2;
		pg.es.push_back(de);

		pg.vs[f1].eadjs.push_back(ind);
		pg.vs[f2].eadjs.push_back(ind);
		ind++;
	}
	pg.eNum = pg.es.size();

	//find sources;
	std::set<int> sources;
	{
		for (unsigned i = 0; i < crestEdgesVisible.size(); i++)
		{
			if (!crestEdgesVisible[i])
				continue;

			unsigned faceID = crestEdges[i][2];
			if (faceID > myMesh->getFaces().size())
			{
				continue; // crest line data is not alway clean
			}
			if (!faceInPatches[faceID]) continue;

			sources.insert(globalIndToLocalInd[faceID]);
		}
		if (!graphCutLocally)
		{
			for (unsigned i = 0; i < userSketches.size(); i++)
			{
				if (!faceInPatches[userSketches[i].fid])continue;

				sources.insert(globalIndToLocalInd[userSketches[i].fid]);
			}
			for (unsigned i = 0; i < fs.size(); i++)
			{
				sources.insert(globalIndToLocalInd[fs[i]]);
			}
		}
	}

	std::vector<double> distanceToFeature;
	ws.distanceToFeature_ShortestPath(pg, sources, distanceToFeature);

	//localhood
	std::vector<std::set<unsigned> > neighbours(pg.vNum);
	for (auto f = myMesh->getFaces().begin(); f != myMesh->getFaces().end(); f++)
	{
		if (faceInPatches[f->id()] == false)continue;

		auto& fs = neighbours[globalIndToLocalInd[f->id()]];
		for (unsigned j = 0; j < 3; j++)
		{
			for (auto e = 0; e < f->vertex_iter(j)->edge_iter().size(); e++)
			{
				unsigned tid = f->vertex_iter(j)->edge_iter()[e]->face_iter(0)->id();
				if (faceInPatches[tid])
					fs.insert(globalIndToLocalInd[tid]);
				if (f->vertex_iter(j)->edge_iter()[e]->manifold())
				{
					tid = f->vertex_iter(j)->edge_iter()[e]->face_iter(1)->id();
					if (faceInPatches[tid])
						fs.insert(globalIndToLocalInd[tid]);
				}
			}
		}
	}

	std::vector<unsigned> watershedLabels;
	ws.grow2(pg, distanceToFeature, neighbours, watershedLabels);

	double cutCost = 0;
	for (auto e = myMesh->getEdges().begin(); e != myMesh->getEdges().end(); e++)
	{
		if (!e->manifold()) continue;

		if (vertexLabel[e->vertex_iter(0)->id()] != vertexLabel[e->vertex_iter(1)->id()])
			graphFeature.ef[e->id()].isCut = true;
		else
			graphFeature.ef[e->id()].isCut = false;

		int f1 = e->face_iter(0)->id();
		int f2 = e->face_iter(1)->id();

		if (!faceInPatches[f1] && !faceInPatches[f2])
			continue;
		else if (faceInPatches[f1] && !faceInPatches[f2] && watershedLabels[globalIndToLocalInd[f1]] == UINT_MAX)
		{
			graphFeature.ef[e->id()].isCut = true;
			continue;
		}
		else if (!faceInPatches[f1] && faceInPatches[f2] && watershedLabels[globalIndToLocalInd[f2]] == UINT_MAX)
		{
			graphFeature.ef[e->id()].isCut = true;
			continue;
		}

		if (watershedLabels[globalIndToLocalInd[f1]] == UINT_MAX && watershedLabels[globalIndToLocalInd[f2]] == UINT_MAX)
		{
			graphFeature.ef[e->id()].isCut = true;
		}
		else
		{
			graphFeature.ef[e->id()].isCut = false;
		}
	}

	unsigned vNum = myMesh->getVertices().size();
	unsigned eNum = myMesh->getEdges().size();
	vertexLabel.clear(); vertexLabel.resize(vNum);
	segNumber = 0;
	std::vector<bool> visitedVer(vNum, false);
	std::vector<bool> visitedEdge(eNum, false);
	for (int i = 0; i < vNum; i++)
	{
		if (visitedVer[i] == true) continue;

		std::vector<int> frontVer(1, i);
		//propagates vertex with same label, and group them into a subgraph
		while (!frontVer.empty())
		{
			int fv = frontVer.back(); frontVer.pop_back();
			vertexLabel[fv] = segNumber;

			auto v = mg.vs[fv].vadjs.begin();
			auto e = mg.vs[fv].eadjs.begin();
			for (; v != mg.vs[fv].vadjs.end(); v++, e++)
			{
				if (graphFeature.ef[*e].isCut || visitedEdge[*e] == true)
					continue;

				visitedEdge[*e] = true;

				if (visitedVer[*v] == true)
					continue;

				visitedVer[*v] = true;

				frontVer.push_back(*v);
			}
		}
		segNumber++;
	}

	cout << "cluster number:" << segNumber << endl;

	vertexLabelBeforeMerge = vertexLabel;

	//update initial cut graph;
	if (false)
	{
		std::vector<bool> isCut(myMesh->getEdges().size());
		for (auto e = myMesh->getEdges().begin(); e != myMesh->getEdges().end(); e++)
		{
			if (!e->manifold()) continue;

			if (vertexLabelInit[e->vertex_iter(0)->id()] != vertexLabelInit[e->vertex_iter(1)->id()])
				isCut[e->id()] = true;
			else
				isCut[e->id()] = false;

			int f1 = e->face_iter(0)->id();
			int f2 = e->face_iter(1)->id();

			if (!faceInPatches[f1] && !faceInPatches[f2])
				continue;
			else if (faceInPatches[f1] && !faceInPatches[f2] && watershedLabels[globalIndToLocalInd[f1]] == UINT_MAX)
			{
				isCut[e->id()] = true;
				continue;
			}
			else if (!faceInPatches[f1] && faceInPatches[f2] && watershedLabels[globalIndToLocalInd[f2]] == UINT_MAX)
			{
				isCut[e->id()] = true;
				continue;
			}

			if (watershedLabels[globalIndToLocalInd[f1]] == UINT_MAX && watershedLabels[globalIndToLocalInd[f2]] == UINT_MAX)
			{
				isCut[e->id()] = true;
			}
			else
			{
				isCut[e->id()] = false;
			}
		}
		vertexLabelInit.clear(); vertexLabelInit.resize(vNum);
		unsigned initSegNumber = 0;
		visitedVer.clear(); visitedVer.resize(vNum, false);
		visitedEdge.clear(); visitedEdge.resize(eNum, false);
		for (int i = 0; i < vNum; i++)
		{
			if (visitedVer[i] == true) continue;

			std::vector<int> frontVer(1, i);
			//propagates vertex with same label, and group them into a subgraph
			while (!frontVer.empty())
			{
				int fv = frontVer.back(); frontVer.pop_back();
				vertexLabelInit[fv] = initSegNumber;

				auto v = mg.vs[fv].vadjs.begin();
				auto e = mg.vs[fv].eadjs.begin();
				for (; v != mg.vs[fv].vadjs.end(); v++, e++)
				{
					if (isCut[*e] || visitedEdge[*e] == true)
						continue;

					visitedEdge[*e] = true;

					if (visitedVer[*v] == true)
						continue;

					visitedVer[*v] = true;

					frontVer.push_back(*v);
				}
			}
			initSegNumber++;
		}
	}

	//for debugging
	ws.distanceToFeature.clear();
	for (int i = 0; i < myMesh->getFaces().size(); i++)
	{
		if (globalIndToLocalInd.find(i) == globalIndToLocalInd.end())
			ws.distanceToFeature.push_back(0);
		else
			ws.distanceToFeature.push_back(distanceToFeature[globalIndToLocalInd[i]]);
	}
}
void MeshSegment::LMP_Partitioning()
{
	clock_t tstr = clock();

	unsigned vNum = myMesh->getVertices().size();
	unsigned eNum = myMesh->getEdges().size();

	andres::graph::Graph<> graph;
	graph.insertVertices(vNum);
	for (auto e = myMesh->getEdges().begin(); e != myMesh->getEdges().end(); e++)
	{
		graph.insertEdge(e->vertex_iter(0)->id(), e->vertex_iter(1)->id());
	}
	std::vector<double> weights;
	getWeights(edgeWeightParameter, weights);

	std::vector<char> edge_labels(graph.numberOfEdges(), 1);
	//andres::graph::multicut_lifted::greedyAdditiveEdgeContraction(graph, graph, weights, edge_labels);

	std::vector<char> out_labels(graph.numberOfEdges(), 1);
	//andres::graph::multicut_lifted::kernighanLin(graph, graph, weights, edge_labels, out_labels);

	auto& multiCutLabels = vertexLabel;
	auto& multiCutNum = segNumber;
	multiCutLabels.clear(); multiCutLabels.resize(myMesh->getVertices().size());
	multiCutNum = 0;
	std::vector<bool> visitedVer(vNum, false);
	std::vector<bool> visitedEdge(eNum, false);
	for (int i = 0; i < vNum; i++)
	{
		if (visitedVer[i] == true) continue;

		std::vector<int> frontVer(1, i);
		//propagates vertex with same label, and group them into a subgraph
		while (!frontVer.empty())
		{
			int fv = frontVer.back(); frontVer.pop_back();
			multiCutLabels[fv] = multiCutNum;

			auto v = mg.vs[fv].vadjs.begin();
			auto e = mg.vs[fv].eadjs.begin();
			for (; v != mg.vs[fv].vadjs.end(); v++, e++)
			{
				if (out_labels[*e] || visitedEdge[*e] == true)
					continue;

				visitedEdge[*e] = true;

				if (visitedVer[*v] == true)
					continue;

				visitedVer[*v] = true;

				frontVer.push_back(*v);
			}
		}
		multiCutNum++;
	}

	clock_t tinit = clock() - tstr;
	cout << endl << "LMP takes:" << tinit / 1000 << "sec" << tinit % 1000 << "mm to generate " << multiCutNum << " patches." << endl;
	//////////////////////////////////////////////////////////////////////////

	vertexLabelBeforeMerge = vertexLabel;

	double cutCost = 0;
	for (int i = 0; i < eNum; i++)
	{
		if (out_labels[i])
		{
			cutCost += weights[i];
			graphFeature.ef[i].isCut = true;
		}
		else
		{
			graphFeature.ef[i].isCut = false;
		}
	}
	cout << "Cost:" << cutCost << endl;
}


// Copyright 2020 the Autoware Foundation
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
//
// Co-developed by Tier IV, Inc. and Apex.AI, Inc.

#ifndef NDT__UTILS_HPP_
#define NDT__UTILS_HPP_

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <geometry_msgs/msg/transform.hpp>
#include <geometry_msgs/msg/pose.hpp>
#include <Eigen/Eigenvalues>
#include <limits>

namespace autoware
{
namespace localization
{
namespace ndt
{

/// This function will check if the covariance is valid based on its eigenvalues. If the covariance
/// is valid, eigen values smaller than a fraction of the biggest eigen value will be capped to the
/// threshold. Covariance then will be reconstructed from the modified set of eigen values and
/// vectors. This should result in increased numerical stability as stated in [Magnusson 2009].
/// If the covariance is invalid, it will not be updated and false will be returned.
/// \tparam Derived Deduced Eigen Matrix type.
/// \param covariance [in, out] Covariance matrix to get stabilized.
/// \param scaling_factor [in] The ratio between the max. eigen value and the minimum
/// allowed eigenvalue. Default value is 0.01 as suggested in [Magnusson 2009], page 60.
/// \return True if the covariance matrix is valid.
template<typename Derived>
bool try_stabilize_covariance(
  Eigen::MatrixBase<Derived> & covariance,
  typename Derived::PlainMatrix::Scalar scaling_factor = 0.01)
{
  using CovMatrixT = typename Derived::PlainMatrix;
  using ScalarT = typename CovMatrixT::Scalar;
  using IndexT = typename CovMatrixT::Index;
  constexpr auto TOL = std::numeric_limits<ScalarT>::epsilon();
  Eigen::SelfAdjointEigenSolver<CovMatrixT> solver;
  solver.compute(covariance);

  CovMatrixT evecs = solver.eigenvectors();
  typename decltype(solver)::RealVectorType evals = solver.eigenvalues();
  // Cap the minimum eigen values to scale times the largest eigen value.
  const ScalarT max_e_val = *std::max_element(evals.data(), evals.data() + evals.size());
  const ScalarT min_e_val = max_e_val * scaling_factor;
  if (min_e_val < TOL) {
    return false;
  }
  auto stabilized = false;
  for (auto i = IndexT{0}; i < evals.size(); ++i) {
    ScalarT & e_val = evals(i);
    if (e_val < TOL) {
      // Covariance is not full rank.
      return false;
    }
    if (e_val < min_e_val) {
      e_val = min_e_val;
      stabilized = true;
    }
  }
  if (stabilized) {
    covariance = evecs * evals.asDiagonal() * evecs.inverse();
  }
  return true;
}

template<typename T>
using EigenPose = Eigen::Matrix<T, 6U, 1U>;
template<typename T>
using EigenTransform = Eigen::Transform<T, 3, Eigen::Affine, Eigen::ColMajor>;
using RosTransform = geometry_msgs::msg::Transform;
using RosPose = geometry_msgs::msg::Pose;
namespace transform_adapters
{
/// Template function to convert a 6D pose to a transformation matrix.
/// This function should be specialized and implemented for the supported types.
/// \tparam PoseT Pose type.
/// \tparam TransformT Transform type.
/// \param[in] pose pose to convert
/// \param[out] transform resulting transform
template<typename PoseT, typename TransformT>
void pose_to_transform(const PoseT & pose, TransformT & transform);

/// Template function to convert a 6D pose to a transformation matrix.
/// This function should be specialized and implemented for the supported types.
/// \tparam PoseT Pose type.
/// \tparam TransformT Transform type.
/// \param[in] transform resulting transform
/// \param[out] pose pose to convert
template<typename PoseT, typename TransformT>
void transform_to_pose(const TransformT & transform, PoseT & pose);

template<typename T>
void pose_to_transform(
  const EigenPose<T> & pose,
  EigenTransform<T> & transform)
{
  static_assert(std::is_floating_point<T>::value, "Eigen transform should use floating points");
  transform.setIdentity();
  transform.translation() = pose.head(3);
  transform.rotate(Eigen::AngleAxis<T>(pose(3), Eigen::Vector3d::UnitX()));
  transform.rotate(Eigen::AngleAxis<T>(pose(4), Eigen::Vector3d::UnitY()));
  transform.rotate(Eigen::AngleAxis<T>(pose(5), Eigen::Vector3d::UnitZ()));
}

/// Specialization to convert from the eigen pose to the ros transform type.
/// \tparam T Eigen scalar type
template<typename T>
void pose_to_transform(
  const EigenPose<T> & pose,
  RosTransform & transform)
{
  static_assert(std::is_floating_point<T>::value, "Eigen pose should use floating points");
  Eigen::Quaternion<T> eig_rot{Eigen::Quaternion<T>{}.setIdentity()};
  eig_rot.setIdentity();
  eig_rot =
    Eigen::AngleAxis<T>(pose(3), Eigen::Matrix<T, 3, 1>::UnitX()) *
    Eigen::AngleAxis<T>(pose(4), Eigen::Matrix<T, 3, 1>::UnitY()) *
    Eigen::AngleAxis<T>(pose(5), Eigen::Matrix<T, 3, 1>::UnitZ());

  decltype(RosTransform::translation) trans;
  decltype(RosTransform::rotation) rot;

  trans.set__x(pose(0)).set__y(pose(1)).set__z(pose(2));
  transform.set__translation(trans);

  rot.set__x(eig_rot.x()).
  set__y(eig_rot.y()).
  set__z(eig_rot.z()).
  set__w(eig_rot.w());
  transform.set__rotation(rot);
}

/// Specialization to convert from the eigen pose to the ros pose type.
/// `pose_to_transform` template is used as conversion to `RosPose`
/// is identical to conversion to `RosTransform`
/// \tparam T Eigen scalar type
template<typename T>
void pose_to_transform(
  const EigenPose<T> & pose,
  RosPose & ros_pose)
{
  static_assert(std::is_floating_point<T>::value, "Eigen pose should use floating points");
  Eigen::Quaternion<T> eig_rot{Eigen::Quaternion<T>{}.setIdentity()};
  eig_rot =
    Eigen::AngleAxis<T>(pose(3), Eigen::Matrix<T, 3, 1>::UnitX()) *
    Eigen::AngleAxis<T>(pose(4), Eigen::Matrix<T, 3, 1>::UnitY()) *
    Eigen::AngleAxis<T>(pose(5), Eigen::Matrix<T, 3, 1>::UnitZ());

  decltype(RosPose::position) trans;
  decltype(RosTransform::rotation) rot;

  trans.set__x(pose(0)).set__y(pose(1)).set__z(pose(2));
  ros_pose.set__position(trans);

  rot.set__x(eig_rot.x()).
  set__y(eig_rot.y()).
  set__z(eig_rot.z()).
  set__w(eig_rot.w());
  ros_pose.set__orientation(rot);
}

/// Specialization to convert from the ros pose type to an eigen one.
/// \tparam T Eigen scalar type
template<typename T>
void transform_to_pose(const RosTransform & transform, EigenPose<T> & pose)
{
  static_assert(std::is_floating_point<T>::value, "Eigen pose should use floating points");
  const auto & ros_rot = transform.rotation;
  const auto & ros_trans = transform.translation;
  Eigen::Quaternion<T> eig_rot{ros_rot.w, ros_rot.x, ros_rot.y, ros_rot.z};
  pose(0) = ros_trans.x;
  pose(1) = ros_trans.y;
  pose(2) = ros_trans.z;

  const auto rot = eig_rot.matrix().eulerAngles(0, 1, 2);
  pose(3) = rot(0);
  pose(4) = rot(1);
  pose(5) = rot(2);
}

/// Specialization to convert from the ros pose type to an eigen one.
/// `transform_to_pose` template is used as conversion from `RosPose`
/// is identical to conversion from `RosTransform`
/// \tparam T Eigen scalar type
template<typename T>
void transform_to_pose(const RosPose & ros_pose, EigenPose<T> & pose)
{
  static_assert(std::is_floating_point<T>::value, "Eigen pose should use floating points");
  const auto & ros_rot = ros_pose.orientation;
  const auto & ros_trans = ros_pose.position;
  Eigen::Quaternion<T> eig_rot{ros_rot.w, ros_rot.x, ros_rot.y, ros_rot.z};
  pose(0) = ros_trans.x;
  pose(1) = ros_trans.y;
  pose(2) = ros_trans.z;

  const auto rot = eig_rot.matrix().eulerAngles(0, 1, 2);
  pose(3) = rot(0);
  pose(4) = rot(1);
  pose(5) = rot(2);
}

}  // namespace transform_adapters
}  // namespace ndt
}  // namespace localization
}  // namespace autoware

#endif  // NDT__UTILS_HPP_


#include "math/SparseMatrix.h"

/// Disable some warning to compile Eigen with gcc 7.1
#ifdef SPH_GCC
#if __GNUC__ >= 7
#pragma GCC diagnostic ignored "-Wint-in-bool-context"
#pragma GCC diagnostic ignored "-Wduplicated-branches"
#endif
#endif

#ifdef SPH_USE_EIGEN
#include <Eigen/IterativeLinearSolvers>
#include <Eigen/SparseLU>
#endif

NAMESPACE_SPH_BEGIN

#ifdef SPH_USE_EIGEN

class SparseMatrix::Impl {
private:
    Eigen::SparseMatrix<Float> matrix;
    Array<Eigen::Triplet<Float>> triplets;

    using SparseVector = Eigen::Matrix<Float, Eigen::Dynamic, 1>;

public:
    Impl(const Size rows, const Size cols)
        : matrix(rows, cols) {}

    void insert(const Size i, const Size j, const Float value) {
        SPH_ASSERT(i < matrix.innerSize() && j < matrix.innerSize(), i, j);
        triplets.push(Eigen::Triplet<Float>(i, j, value));
    }

    Expected<Array<Float>> solve(const Array<Float>& values,
        const SparseMatrix::Solver solver,
        const Float tolerance) {
        SPH_ASSERT(values.size() == matrix.innerSize());
        // this is takes straigh from Eigen documentation
        // (http://eigen.tuxfamily.org/dox-devel/group__TopicSparseSystems.html)

        matrix.setFromTriplets(triplets.begin(), triplets.end());
        /// \todo experiment with different solvers

        SparseVector b;
        b.resize(values.size());
        for (Size i = 0; i < values.size(); ++i) {
            b(i) = values[i];
        }

        Expected<SparseVector> a;
        switch (solver) {
        case SparseMatrix::Solver::LU: {
            Eigen::SparseLU<Eigen::SparseMatrix<Float>, Eigen::COLAMDOrdering<int>> solver;
            a = solveImpl(solver, b);
            break;
        }
        case SparseMatrix::Solver::CG: {
#ifdef SPH_DEBUG
            // check that the matrix is symmetric
            {
                const Size n = matrix.innerSize();
                for (Size i = 0; i < n; ++i) {
                    for (Size j = i; j < n; ++j) {
                        const Float mij = matrix.coeff(i, j);
                        const Float mji = matrix.coeff(j, i);
                        SPH_ASSERT(almostEqual(mij, mji), mij, mji);
                    }
                }
            }
#endif
            Eigen::ConjugateGradient<Eigen::SparseMatrix<Float>, Eigen::Lower | Eigen::Upper> solver;
            if (tolerance > 0._f) {
                solver.setTolerance(tolerance);
            }
            a = solveImpl(solver, b);
            break;
        }
        case SparseMatrix::Solver::LSCG: {
            Eigen::LeastSquaresConjugateGradient<Eigen::SparseMatrix<Float>> solver;
            if (tolerance > 0._f) {
                solver.setTolerance(tolerance);
            }
            a = solveImpl(solver, b);
            break;
        }
        case SparseMatrix::Solver::BICGSTAB: {
            Eigen::BiCGSTAB<Eigen::SparseMatrix<Float>> solver;
            if (tolerance > 0._f) {
                solver.setTolerance(tolerance);
            }
            a = solveImpl(solver, b);
            break;
        }
        default:
            NOT_IMPLEMENTED;
        }

        if (!a) {
            return makeUnexpected<Array<Float>>(a.error());
        }
        Array<Float> result;
        result.resize(matrix.outerSize());
        for (Size i = 0; i < result.size(); ++i) {
            result[i] = a.value()(i);
        }
        return Expected<Array<Float>>(std::move(result));
    }

private:
    template <typename TSolver>
    Expected<SparseVector> solveImpl(TSolver& solver, const SparseVector& b) {
        solver.compute(matrix);
        if (solver.info() != Eigen::Success) {
            return makeUnexpected<SparseVector>("Decomposition of matrix failed");
        }
        SparseVector a;
        a = solver.solve(b);
        if (solver.info() != Eigen::Success) {
            return makeUnexpected<SparseVector>("Equations cannot be solved");
        }
        return a;
    }
};

SparseMatrix::SparseMatrix() = default;

SparseMatrix::SparseMatrix(const Size rows, const Size cols)
    : impl(makeAuto<Impl>(rows, cols)) {}

SparseMatrix::~SparseMatrix() = default;

void SparseMatrix::resize(const Size rows, const Size cols) {
    impl = makeAuto<Impl>(rows, cols);
}

void SparseMatrix::insert(const Size i, const Size j, const Float value) {
    impl->insert(i, j, value);
}

Expected<Array<Float>> SparseMatrix::solve(const Array<Float>& values,
    const Solver solver,
    const Float tolerance) {
    return impl->solve(values, solver, tolerance);
}

#endif

NAMESPACE_SPH_END


#ifndef MANIFOLDS_FUNCTIONS_MULTI_MATRIX_REDUCTION_HH
#define MANIFOLDS_FUNCTIONS_MULTI_MATRIX_REDUCTION_HH

#include "data/multi_matrix.hh"
#include "function.hh"
#include "full_function_defs.hh"
#include <boost/mpl/find.hpp>
#include <boost/type_traits/is_same.hpp>
#include <boost/mpl/end.hpp>
#include <utility>

namespace manifolds {
template <int n1, int n2> struct ReduxPair {
  static const int first = n1;
  static const int second = n2;
};

template <class... ReductionPairs>
struct Reduction : Function<int_<26>, 1, 1>,
                   FunctionCommon<Reduction<ReductionPairs...> > {
  using FunctionCommon<Reduction>::operator();
  template <class Arg, class Array, class... Indices,
            class = typename std::enable_if<
                (sizeof...(Indices) == Arg::dimensions)>::type>
  static auto CoeffFromArray(void *, Arg arg, Array a, Indices... indices) {
    return arg.Coeff(indices...);
  }

  template <class Arg, class Array, class... Indices,
            class = typename std::enable_if<
                (sizeof...(Indices) != Arg::dimensions)>::type>
  static auto CoeffFromArray(int *, Arg arg, Array a, Indices... indices) {
    return CoeffFromArray(arg, a, indices..., a[sizeof...(Indices)]);
  }

  template <class Vector, int index>
  using is_not_in = typename boost::is_same<
      typename boost::mpl::find<Vector, int_<index> >::type,
      typename boost::mpl::end<Vector>::type>::type;

  template <class MMatrix, class Skips, class IndicesTuple = tuple<>,
            int index = 0, bool = MMatrix::dimensions == index>
  struct output_type {
    typedef int_<MMatrix::template dimension<index>::value> dim;
    typedef typename std::conditional<
        is_not_in<Skips, index>::value,
        decltype(push_back(std::declval<IndicesTuple>(), dim())),
        IndicesTuple>::type next_tuple;

    typedef typename output_type<MMatrix, Skips, next_tuple, index + 1>::type
    type;
  };

  template <class MMatrix, class Skips, int index, int... dimensions>
  struct output_type<MMatrix, Skips, tuple<int_<dimensions>...>, index, true> {
    typedef MultiMatrix<typename MMatrix::CoefficientType, dimensions...> type;
  };

  template <class Array> static bool inc_pair(Array &a, int b, int c, int dim) {
    ++a[b];
    ++a[c];
    return a[b] == dim;
  }

  template <class Arg> auto eval(Arg arg) const {
    static const bool all_good = and_<bool_<
        Arg::template dimension<ReductionPairs::first>::value ==
        Arg::template dimension<ReductionPairs::second>::value>...>::value;
    static_assert(all_good, "Can only contract indices "
                            "of the same dimension");
    static std::array<std::pair<int, int>, sizeof...(ReductionPairs)> reduxes =
        { { std::make_pair(ReductionPairs::first,
                           ReductionPairs::second)... } };
    int in_indices[Arg::dimensions] = {};
    typename output_type<
        Arg, boost::mpl::vector<int_<ReductionPairs::first>...,
                                int_<ReductionPairs::second>...> >::type result;
    int out_indices[decltype(result)::dimensions] = {};
    while (true) {
      while (true) {
        result.Coeff(out_indices) += arg.Coeff(in_indices);
        unsigned index = 0;
        while (inc_pair(in_indices, reduxes[index].first, reduxes[index].second,
                        arg.Dimension(reduxes[index].second))) {
          in_indices[reduxes[index].first] = 0;
          in_indices[reduxes[index].second] = 0;
          if (++index == sizeof...(ReductionPairs)) {
            goto output_index_done;
          }
        }
      }
    output_index_done:
      int index = 0;
      while (true) {
        if (std::find_if(reduxes.begin(), reduxes.end(), [index](auto x) {
              return x.first == index || x.second == index;
            }) == reduxes.end()) {
          if (++in_indices[index] == arg.Dimension(index)) {
            int smaller_ones = 0;
            for (auto x : reduxes) {
              smaller_ones += ((x.first) < index) + (x.second < index);
            }
            in_indices[index] = out_indices[index - smaller_ones] = 0;
            if (++index == (int)Arg::dimensions)
              return result;
          } else {
            int smaller_ones = 0;
            for (auto x : reduxes) {
              smaller_ones += ((x.first) < index) + (x.second < index);
            }
            ++out_indices[index - smaller_ones];
            break;
          }
        } else if (++index == (int)Arg::dimensions)
          return result;
      }
    }
  }
};
}

#endif