/*
 * func_ver.cpp
 *
 *  Created on: 04.07.2012
 *      Author: stephaniebayer
 */

#include "func_ver.h"
#include<vector>
#include "Cipher_elg.h"
#include "G_q.h"
#include "Mod_p.h"
#include "Functions.h"
#include "ElGammal.h"
#include "multi_expo.h"
#include <fstream>

#include <time.h>
#include <NTL/ZZ.h>
NTL_CLIENT

extern G_q G;
extern G_q H;
extern Pedersen Ped;
extern ElGammal El;

func_ver::func_ver() {
	// TODO Auto-generated constructor stub

}

func_ver::~func_ver() {
	// TODO Auto-generated destructor stub
}

int func_ver::check_Dh(vector<Mod_p>* c_Dh, vector<ZZ>* chal, vector<ZZ>* D_h_bar, ZZ r_Dh_bar){
	long i;
	Mod_p t_Dh, co_Dh, temp;
	long m = c_Dh->size();

	t_Dh =c_Dh->at(0);
	for (i=1; i<m;i++){
		Mod_p::expo(temp,c_Dh->at(i),chal->at(i-1));
		Mod_p::mult(t_Dh,t_Dh,temp);
	}
	co_Dh = Ped.commit(D_h_bar,r_Dh_bar);
/*	string name = "example.txt";
	ofstream ost;
	ost.open(name.c_str(),ios::app);
	ost<<"D_h "<<t_Dh<<" "<<co_Dh<<endl;*/
	if (t_Dh == co_Dh){
		return 1;
	}
	return 0;
}

int func_ver::check_D(Mod_p c_D0, Mod_p c_z, vector<Mod_p>* c_A, vector<Mod_p>* c_B, vector<ZZ>* chal_1, ZZ chal_2, vector<ZZ>* A_bar, ZZ r_A_bar, long n){
	int i;
	Mod_p t_D, co_D, temp, inv;
	long m = c_A->size()-1;
	ZZ ord = H.get_ord();
	vector<ZZ>* v_1 = new vector<ZZ>(n);

	t_D = c_D0;
	Mod_p::inv(inv, c_z);
	for (i=1; i<m;i++){
		Mod_p::expo(temp,c_A->at(i),chal_2);
		Mod_p::mult(temp, temp,c_B->at(i));
		Mod_p::mult(temp,temp,inv);
		Mod_p::expo(temp, temp, chal_1->at(i-1));
		Mod_p::mult(t_D,t_D,temp);
	}
	for(i=0; i<n;i++){
		NegateMod(v_1->at(i),to_ZZ(1),ord);
	}
	temp=Ped.commit(v_1,to_ZZ(0));
	Mod_p::expo(temp, temp, chal_1->at(m-1));
	Mod_p::mult(t_D,t_D,temp);
	co_D = Ped.commit(A_bar, r_A_bar);

	/*string name = "example.txt";
	ofstream s;
	ost.open(name.c_str(),ios::app);
	ost<<"D "<<t_D<<" "<<co_D<<endl;*/
	delete v_1;
	if(t_D == co_D){
		return 1;
	}
	return 0;
}

int func_ver::check_Ds(vector<Mod_p>* c_Ds, vector<Mod_p>* c_Dh, Mod_p c_Dm, vector<ZZ>* chal_1, vector<ZZ>* chal_2, vector<ZZ>* Ds_bar, ZZ r_Ds_bar){
	long i,l;
	long m= c_Ds->size()-1;
	Mod_p t_Ds, co_Ds, temp, temp_1;

	l=m-1;
	for(i=0; i<l; i++){
		Mod_p::expo(c_Ds->at(i),c_Dh->at(i), chal_1->at(i));
	}
	if(m>1){
		Mod_p::expo(temp, c_Dh->at(1), chal_1->at(0));
		for(i=1;i<l; i++){
			Mod_p::expo(temp_1,c_Dh->at(i+1), chal_1->at(i));
			Mod_p::mult(temp,temp,temp_1);
		}
		c_Ds->at(l)=temp;
	}
	else{
		c_Ds->at(l)=Mod_p(1,G.get_mod());
	}

	c_Ds->at(m)=c_Dm;
	Mod_p::expo(t_Ds, c_Ds->at(0),chal_2->at(m-1));
	for(i=1; i<m; i++){
		Mod_p::expo(temp, c_Ds->at(i), chal_2->at(m-1-i));
		Mod_p::mult(t_Ds, t_Ds,temp);
	}
	Mod_p::mult(t_Ds,t_Ds,c_Ds->at(m));
	co_Ds = Ped.commit(Ds_bar, r_Ds_bar);

/*	string name = "example.txt";
	ofstream ost;
	ost.open(name.c_str(),ios::app);
	ost<<"Ds "<<t_Ds<<" "<<co_Ds<<endl;*/
	if(t_Ds == co_Ds){
		return 1;
	}
	return 0;
}

int func_ver::check_Dl(vector<Mod_p>* c_Dl, vector<ZZ>* chal_1, vector<ZZ>* A, vector<ZZ>* B, vector<ZZ>*  chal_2, ZZ r_Dl_bar){
	long i;
	Mod_p t_Dl, co_Dl, temp;
	ZZ temp_1;
	long l = c_Dl->size();
	long pos = (l-1)/2+1;
	ZZ mod = G.get_mod();
	t_Dl = c_Dl->at(0);
	for(i=1; i<l; i++){
		Mod_p::expo(temp, c_Dl->at(i),chal_1->at(i-1));
		Mod_p::mult(t_Dl, t_Dl, temp);
	}
	temp_1=Functions::bilinearMap(A,B,chal_2);
	co_Dl =Ped.commit(temp_1,r_Dl_bar);

/*	string name = "example.txt";
	ofstream ost;
	ost.open(name.c_str(),ios::app);
	ost<<"Dl "<<t_Dl<<" "<<co_Dl<<endl;*/

	temp = Mod_p(1,mod);
	if(t_Dl==co_Dl & c_Dl->at(pos) ==temp){
		return 1;
	}
	return 0;
}

int func_ver::check_d(vector<Mod_p>* c_Dh, Mod_p c_d, vector<ZZ>* chal, vector<ZZ>* d_bar, ZZ r_d_bar){
	Mod_p t_d, co_d, temp;
	long m = c_Dh->size();

	Mod_p::expo(temp, c_Dh->at(m-1), chal->at(0));
	Mod_p::mult(t_d, temp, c_d);
		co_d = Ped.commit(d_bar, r_d_bar);

	/*	string name = "example.txt";
		ofstream ost;
		ost.open(name.c_str(),ios::app);
		ost<<"d "<<t_d<<" "<<co_d<<endl;*/
	if(t_d==co_d){
		return 1;
	}
	return 0;
}

int func_ver::check_Delta(Mod_p c_dh, Mod_p c_Delta, vector<ZZ>* chal, vector<ZZ>* Delta_bar, vector<ZZ>* d_bar, ZZ r_Delta_bar, ZZ chal_1, ZZ chal_2, ZZ chal_3){
	long i,j;
	Mod_p t_Delta, co_Delta, temp;
	ZZ t_1, t_2, t_3, prod, chal_temp;
	ZZ ord = H.get_ord();
	long m = (chal->size()-1)/2;
	long n = Delta_bar->size();
	vector<ZZ>* Delta_temp=0;

	Mod_p::expo(temp, c_dh, chal->at(0));
	Mod_p::mult(t_Delta, temp, c_Delta);

	Delta_temp = new vector<ZZ>(n-1);
	t_3= chal->at(0);
	for(i=0; i<n-1; i++){
		MulMod(t_1, Delta_bar->at(i), d_bar->at(i+1), ord);
		MulMod(t_2, t_3, Delta_bar->at(i+1),ord);
		SubMod(Delta_temp->at(i), t_2, t_1, ord);
	}

	co_Delta = Ped.commit(Delta_temp, r_Delta_bar);

	delete Delta_temp;
/*	string name = "example.txt";
	ofstream ost;
	ost.open(name.c_str(),ios::app);
	ost<<"Delta "<<t_Delta<<" "<<c_Delta<<endl;*/

	prod = to_ZZ(1);
	chal_temp =to_ZZ(1);
	for(i=1; i<=m; i++){
		for(j=1; j<=n; j++){
			MulMod(chal_temp, chal_temp, chal_1,ord);
			SubMod(t_1, chal_temp, chal_2,ord);
			t_3 = n*(i-1)+j;
			MulMod(t_3,t_3, chal_3, ord);
			AddMod(t_1,t_1, t_3, ord);
			MulMod(prod,prod, t_1, ord);
		}
	}
	MulMod(prod, prod, chal->at(0), ord);

	//	ost<<"prod "<<prod<<" "<<Delta_bar->at(n-1)<<endl;
	//ost<<d_bar->at(0)<<" "<<Delta_bar->at(0)<<endl;
	if(t_Delta ==co_Delta)
		if(prod ==Delta_bar->at(n-1) & d_bar->at(0)==Delta_bar->at(0)){
		return 1;
	}
	return 0;
}

int func_ver::check_B(vector<Mod_p>* c_B, Mod_p c_B0, vector<ZZ>* chal, vector<ZZ>* B_bar, ZZ r_B_bar){
	long i;
	Mod_p t_B, co_B, temp, temp_1;
	long m = c_B->size();
	//check for correctness of the committed B
	temp = c_B0;
	for(i = 0; i<m; i++){
		Mod_p::expo(temp_1,c_B->at(i), chal->at(i));
		Mod_p::mult(temp,temp,temp_1);
	}
	t_B = temp;
	co_B = Ped.commit(B_bar, r_B_bar);
/*	string name = "example.txt";
	ofstream ost;
	ost.open(name.c_str(),ios::app);
	ost<<"B "<<t_B<<" "<<co_B<<endl;*/
	if(t_B==co_B){
		return 1;
	}
	return 0;
}

int func_ver::check_a(vector<Mod_p>* c_a, vector<Mod_p>* c_Dl, vector<ZZ>* chal, ZZ a_bar, ZZ r_a_bar){
	long i,l;
	Mod_p t_a, co_a, temp, temp_1;
	long m = c_a->size()/2;

	//Check that the random values are used right
	temp = c_a->at(0);
	l=2*m-1;
	for(i = 1; i<=l; i++){
		Mod_p::expo(temp_1,c_a->at(i), chal->at(i-1));
		Mod_p::mult(temp,temp,temp_1);
	}
	t_a = temp;
	co_a = Ped.commit(a_bar, r_a_bar);
/*	string name = "example.txt";
	ofstream ost;
	ost.open(name.c_str(),ios::app);
	ost<<"a "<<t_a<<" "<<co_a<<endl;
*/
	if(t_a==co_a & c_a->at(m)==c_Dl->at(m+1)){//both commitments should be com(0,0), so equal
		return 1;
	}
	return 0;
}

int func_ver::check_c(vector<vector<Cipher_elg>* >* enc, vector<Cipher_elg>* E, ZZ chal){
	long i,j;
	ZZ chal_temp;
	ZZ ord = H.get_ord();
	Cipher_elg c, temp;
	long m = enc->size();
	long n = enc->at(0)->size();

	chal_temp = to_ZZ(1);
	c = Cipher_elg(1,1,H.get_mod());
 	for(i = 0; i<m;i++){
		for(j = 0; j<n;j++){
			MulMod(chal_temp, chal_temp, chal, ord);
			Cipher_elg::expo(temp,enc->at(i)->at(j),chal_temp);
			Cipher_elg::mult(c,c,temp);
		}
	}
 	if(E->at(m)==c){
 		return 1;
 	}
 	return 0;
}

int func_ver::check_E(vector<vector<Cipher_elg>* >* C, vector<Cipher_elg>* E, vector<ZZ>* chal, vector<ZZ>* B_bar, ZZ a_bar, ZZ rho_bar ){
	long i,j,l;
	Cipher_elg t_E, co_E, temp, temp_1, temp_2;
	Mod_p gen,t;
	long m = C->size();
	long n = C->at(0)->size();
	ZZ te;
	ZZ ord = H.get_ord();
	ZZ mod = H.get_mod();

 	temp = E->at(0);
 	l=2*m;
	for(i = 1; i<l; i++){
		Cipher_elg::expo(temp_1,E->at(i), chal->at(i-1));
		Cipher_elg::mult(temp,temp,temp_1);
	}
	t_E = temp;

	gen = H.get_gen();
	t = Mod_p::expo(gen,a_bar);
	temp = El.encrypt(t, rho_bar);
	temp_1=Cipher_elg(1,1,mod);
	l=m-1;
	for(i = 0; i<l;i++){
		for(j = 0; j<n;j++){
			MulMod(te , B_bar->at(j),chal->at(m-i-2),ord);
			Cipher_elg::expo(temp_2,C->at(i)->at(j),te);
			Cipher_elg::mult(temp_1,temp_1,temp_2);
		}
	}
	for(j = 0; j<n;j++){
		te =B_bar->at(j);
		Cipher_elg::expo(temp_2,C->at(m-1)->at(j),te);
		Cipher_elg::mult(temp_1,temp_1,temp_2);
	}
	Cipher_elg::mult(co_E ,temp_1, temp);

/*	string name = "example.txt";
	ofstream ost;
	ost.open(name.c_str(),ios::app);
	ost<<"E"<<t_E<<" "<<co_E<<endl;*/
	if(t_E==co_E){
		return 1;
	}
	return 0;
}

int func_ver::check_Dh_op(vector<Mod_p>* c_Dh, vector<ZZ>* chal, vector<ZZ>* D_h_bar, ZZ r_Dh_bar, long win_LL){
	Mod_p t_Dh, co_Dh;

	multi_expo::multi_expo_LL(t_Dh,c_Dh, chal, win_LL);
	co_Dh = Ped.commit_opt(D_h_bar,r_Dh_bar);

	//cout<<"D_h "<<t_Dh<<" "<<co_Dh<<endl;
	if (t_Dh == co_Dh){
		return 1;
	}
	return 0;
}

int func_ver::check_D_op(Mod_p c_D0, Mod_p c_z, vector<Mod_p>* c_A, vector<Mod_p>* c_B, vector<ZZ>* chal_1, ZZ chal_2, vector<ZZ>* A_bar, ZZ r_A_bar, long n){
	int i;
	Mod_p t_D, co_D, temp, inv;
	long m = c_A->size()-1;
	ZZ ord = H.get_ord();
	vector<ZZ>* v_1 = new vector<ZZ>(n);

	t_D = c_D0;
	Mod_p::inv(inv, c_z);
	for (i=1; i<m;i++){
		Mod_p::expo(temp,c_A->at(i),chal_2);
		Mod_p::mult(temp, temp,c_B->at(i));
		Mod_p::mult(temp,temp,inv);
		Mod_p::expo(temp, temp, chal_1->at(i-1));
		Mod_p::mult(t_D,t_D,temp);
	}
	for(i=0; i<n;i++){
		NegateMod(v_1->at(i),to_ZZ(1),ord);
	}
	temp=Ped.commit_opt(v_1,to_ZZ(0));
	Mod_p::expo(temp, temp, chal_1->at(m-1));
	Mod_p::mult(t_D,t_D,temp);
	co_D = Ped.commit_opt(A_bar, r_A_bar);
	//cout<<"D "<<t_D<<" "<<co_D<<endl;
	delete v_1;
	if(t_D == co_D){
		return 1;
	}
	return 0;
}

int func_ver::check_Ds_op(vector<Mod_p>* c_Ds, vector<Mod_p>* c_Dh, Mod_p c_Dm, vector<ZZ>* chal_1, vector<ZZ>* chal_2, vector<ZZ>* Ds_bar, ZZ r_Ds_bar){
	long i,l;
	long m= c_Ds->size()-1;
	Mod_p t_Ds, co_Ds, temp, temp_1;

	l=m-1;
	for(i=0; i<l; i++){
		Mod_p::expo(c_Ds->at(i),c_Dh->at(i), chal_1->at(i));
	}
	if(m>1){
		Mod_p::expo(temp, c_Dh->at(1), chal_1->at(0));
		for(i=1;i<l; i++){
			Mod_p::expo(temp_1,c_Dh->at(i+1), chal_1->at(i));
			Mod_p::mult(temp,temp,temp_1);
		}
		c_Ds->at(l)=temp;
	}
	else{
		c_Ds->at(l)=Mod_p(1,G.get_mod());
	}

	c_Ds->at(m)=c_Dm;
	Mod_p::expo(t_Ds, c_Ds->at(0),chal_2->at(m-1));
	for(i=1; i<m; i++){
		Mod_p::expo(temp, c_Ds->at(i), chal_2->at(m-1-i));
		Mod_p::mult(t_Ds, t_Ds,temp);
	}
	Mod_p::mult(t_Ds,t_Ds,c_Ds->at(m));
	co_Ds = Ped.commit_opt(Ds_bar, r_Ds_bar);
	//cout<<"Ds "<<t_Ds<<" "<<co_Ds<<endl;
	if(t_Ds == co_Ds){
		return 1;
	}
	return 0;
}

int func_ver::check_Dl_op(vector<Mod_p>* c_Dl, vector<ZZ>* chal, vector<ZZ>* A_bar, vector<ZZ>* Ds_bar, vector<ZZ>*  chal_1, ZZ r_Dl_bar){
	long i;
	Mod_p t_Dl, co_Dl, temp;
	ZZ temp_1;
	long l = c_Dl->size();
	long pos = (l-1)/2+1;
	ZZ mod = G.get_mod();

	t_Dl = c_Dl->at(0);
	for(i=1; i<l; i++){
		Mod_p::expo(temp, c_Dl->at(i),chal->at(i-1));
		//Mod_p::mult(t_Dl,t_Dl, temp);
		t_Dl = t_Dl*temp;
	}
	temp_1=Functions::bilinearMap(A_bar,Ds_bar,chal_1);
	co_Dl =Ped.commit_sw(temp_1,r_Dl_bar);
	//cout<<"Dl "<<t_Dl<<" "<<co_Dl<<endl;

	temp= Mod_p(1, mod);
	if(t_Dl==co_Dl & c_Dl->at(pos)==temp){
		return 1;
	}
	return 0;
}

int func_ver::check_d_op(vector<Mod_p>* c_Dh, Mod_p c_d, vector<ZZ>* chal, vector<ZZ>* d_bar, ZZ r_d_bar){
	Mod_p t_d, co_d, temp;
	long m = c_Dh->size();
	Mod_p::expo(temp, c_Dh->at(m-1), chal->at(0));
	Mod_p::mult(t_d, temp, c_d);
	co_d = Ped.commit_opt(d_bar, r_d_bar);
	//cout<<"d "<<t_d<<" "<<co_d<<endl;
	if(t_d==co_d){
		return 1;
	}
	return 0;
}

int func_ver::check_Delta_op(Mod_p c_dh, Mod_p c_Delta, vector<ZZ>* chal, vector<ZZ>* Delta_bar, vector<ZZ>* d_bar, ZZ r_Delta_bar, ZZ chal_1, ZZ chal_2, ZZ chal_3){
	long i,j;
	Mod_p t_Delta, co_Delta, temp;
	ZZ t_1, t_2, t_3, prod, chal_temp;
	ZZ ord = H.get_ord();
	long m = (chal->size()-1)/2;
	long n = Delta_bar->size();
	vector<ZZ>* Delta_temp=0;

	Mod_p::expo(temp, c_dh, chal->at(0));
	Mod_p::mult(t_Delta, temp, c_Delta);

	Delta_temp = new vector<ZZ>(n-1);
	t_3= chal->at(0);
	for(i=0; i<n-1; i++){
		MulMod(t_1, Delta_bar->at(i), d_bar->at(i+1), ord);
		MulMod(t_2, t_3, Delta_bar->at(i+1),ord);
		SubMod(Delta_temp->at(i), t_2, t_1, ord);
	}

	co_Delta = Ped.commit_opt(Delta_temp, r_Delta_bar);

	delete Delta_temp;
	//cout<<"Delta "<<t_Delta<<" "<<co_Delta<<endl;

	prod = to_ZZ(1);
	chal_temp =to_ZZ(1);
	for(i=1; i<=m; i++){
		for(j=1; j<=n; j++){
			MulMod(chal_temp, chal_temp, chal_1,ord);
			SubMod(t_1, chal_temp, chal_2,ord);
			t_3 = n*(i-1)+j;
			MulMod(t_3,t_3, chal_3, ord);
			AddMod(t_1,t_1, t_3, ord);
			MulMod(prod,prod, t_1, ord);
		}
	}
	MulMod(prod, prod, chal->at(0), ord);

	//cout<<"prod "<<prod<<" "<<Delta_bar->at(n-1)<<endl;
	//cout<<d_bar->at(0)<<" "<<Delta_bar->at(0)<<endl;
	if(t_Delta ==co_Delta)
		if(prod ==Delta_bar->at(n-1) & d_bar->at(0)==Delta_bar->at(0)){
		return 1;
	}
	return 0;
}

int func_ver::check_B_op(vector<Mod_p>* c_B, Mod_p c_B0, vector<ZZ>* chal, vector<ZZ>* B_bar, ZZ r_B_bar, long win_LL){
	long i;
	Mod_p t_B, co_B, temp, temp_1;
	long m = c_B->size();
	vector<Mod_p>* B_temp = new vector<Mod_p>(m+1);
	vector<ZZ>* chal_mult = new vector<ZZ>(m+1);

	//check for correctness of the committed B
	B_temp ->at(0)= c_B0;
	chal_mult->at(0)=1;
	for(i = 0; i<m; i++){
		B_temp->at(i+1)=c_B->at(i);
		chal_mult->at(i+1)=chal->at(i);
	}
	multi_expo::multi_expo_LL(t_B, B_temp, chal_mult, win_LL);

	co_B = Ped.commit_opt(B_bar, r_B_bar);
	//cout<<"B "<<t_B<<" "<<co_B<<endl;
	delete B_temp;
	delete chal_mult;
	if(t_B==co_B){
		return 1;
	}
	return 0;
}

int func_ver::check_a_op(vector<Mod_p>* c_a, vector<Mod_p>* c_Dl, vector<ZZ>* chal, ZZ a_bar, ZZ r_a_bar){
	long i,l;
	Mod_p t_a, co_a, temp, temp_1;
	long m = c_a->size()/2;

	//Check that the random values are used right
	temp = c_a->at(0);
	l=2*m-1;
	for(i = 1; i<=l; i++){
		Mod_p::expo(temp_1,c_a->at(i), chal->at(i-1));
		Mod_p::mult(temp,temp,temp_1);
	}
	t_a = temp;
	co_a = Ped.commit_sw(a_bar, r_a_bar);
	//cout<<"a "<<t_a<<" "<<co_a<<endl;

	if(t_a==co_a & c_a->at(m)==c_Dl->at(m+1)){//both commitments should be com(0,0), so equal
		return 1;
	}
	return 0;
}

int func_ver::check_c_op(vector<vector<Cipher_elg>* >* enc, vector<Cipher_elg>* E, ZZ chal, long omega){
	long i,j;
	ZZ chal_temp;
	ZZ ord = H.get_ord();
	Cipher_elg c, temp;
	vector<ZZ>* v_chal = 0;
	long m = enc->size();
	long n = enc->at(0)->size();

	chal_temp = to_ZZ(1);
	c = Cipher_elg(1,1,H.get_mod());
	v_chal = new vector<ZZ>(n);
 	for(i = 0; i<m;i++){
		for(j = 0; j<n;j++){
			MulMod(chal_temp, chal_temp, chal, ord);
			v_chal->at(j)=chal_temp;
		}
		multi_expo::expo_mult(temp, enc->at(i), v_chal, omega);
		Cipher_elg::mult(c,c,temp);
	}
 	delete v_chal;
 	//cout<<"c "<<E->at(m)<<" "<<c<<endl;
 	if(E->at(m)==c){
 		return 1;
 	}
 	return 0;
}

int func_ver::check_E_op(vector<vector<Cipher_elg>* >* C, vector<Cipher_elg>* E, vector<ZZ>* chal, vector<ZZ>* B_bar, ZZ a_bar, ZZ rho_bar , long omega){
	long i,l;
	Cipher_elg t_E, co_E, temp, temp_1;
	Mod_p gen,t;
	long m = C->size();
	ZZ ord = H.get_ord();
	ZZ mod = H.get_mod();
	long num_b = NumBits(ord);
	l=2*m;
	vector<vector<long>* >* basis_chal = new vector<vector<long>* >(l);

	basis_chal ->at(0)= multi_expo::to_basis(to_ZZ(1), num_b, omega);
	basis_chal->at(1) = multi_expo::to_basis(chal->at(0), num_b, omega);
	for(i=2; i<l; i++){
		basis_chal->at(i) = multi_expo::to_basis(chal->at(i-1), num_b, omega);
	}

	multi_expo::expo_mult(t_E, E, basis_chal, omega);

	vector<ZZ>* chal_temp = new vector<ZZ>(m);
	l=m-1;
	for(i=0; i<l; i++){
		chal_temp->at(i)= chal->at(m-i-2);
	}
	chal_temp->at(l)=1;

	gen = H.get_gen();
	t = Mod_p::expo(gen,a_bar);
	temp = El.encrypt(t, rho_bar);
	multi_expo::expo_mult(temp_1, C, chal_temp, B_bar, omega);

	Cipher_elg::mult(co_E ,temp_1, temp);

	Functions::delete_vector(basis_chal);
	delete(chal_temp);
	//cout<<"E"<<t_E<<" "<<co_E<<endl;
	if(t_E==co_E){
		return 1;
	}

	return 0;

}

void func_ver::fill_vector(vector<ZZ>* t){
	long i,l;
	ZZ temp;
	ZZ ord = H.get_ord();

	l= t->size();
	temp = RandomBnd(ord);
	t->at(0)=temp;
	for(i=1; i<l; i++){
		MulMod(t->at(i),t->at(i-1),temp, ord);
	}
}

void func_ver::fill_x8(vector<ZZ>* chal_x8, vector<vector<long>* >* basis_chal_x8, vector<ZZ>* mul_chal_x8, long omega){
	long i, l;
	ZZ chal;
	ZZ ord = H.get_ord();
	long num_b= NumBits(ord);

	l= chal_x8->size();
	chal = RandomBnd(ord);

	chal_x8->at(0)= chal;
	basis_chal_x8->at(0) = multi_expo::to_basis(to_ZZ(1),num_b, omega);
	basis_chal_x8->at(1) = multi_expo::to_basis(chal_x8->at(0),num_b, omega);

	mul_chal_x8->at(0) =1;
	mul_chal_x8->at(1) =chal_x8->at(0);

	for (i = 1; i<l; i++){
		 MulMod(chal_x8->at(i),chal, chal_x8->at(i-1), ord);
		 basis_chal_x8->at(i+1) = multi_expo::to_basis(chal_x8->at(i),num_b, omega);
		 mul_chal_x8->at(i+1) = chal_x8->at(i);
	}
}


#include "pauli_product.cpp"
#include <armadillo>
#include <chrono>
#include <iostream>
#include <stdlib.h>

using namespace std;
using namespace arma;

int main()
{
  int n = 1024;
  int n_itt = 100;

  for (int i=0; i<n_itt; i++)
    {
      cx_vec psi;
      psi.randn(n);
      psi = normalise(psi, 2);

      unsigned x = rand()%n;
      unsigned z = rand()%n;

      double m1 = measure_pp(x, z, psi);
      double m2 = measure_pp_slow(x, z, psi);

      cout << m1 - m2 << endl;
    }

  cx_vec psi;
  psi.randn(n);
  psi = normalise(psi, 2);
  std::chrono::steady_clock::time_point begin = std::chrono::steady_clock::now();
  for (int i=0; i<n_itt; i++)
    {
      unsigned x = rand()%n;
      unsigned z = rand()%n;

      double m = measure_pp(x, z, psi);
    }
  std::chrono::steady_clock::time_point end = std::chrono::steady_clock::now();

  std::cout << "Measurement Time (fast) = " << std::chrono::duration_cast<std::chrono::microseconds>(end - begin).count()/n_itt << "[µs]" << std::endl;
  std::cout << "Measurement Time (fast) = " << std::chrono::duration_cast<std::chrono::nanoseconds> (end - begin).count()/n_itt << "[ns]" << std::endl;

  psi.randn(n);
  psi = normalise(psi, 2);
  std::chrono::steady_clock::time_point begin2 = std::chrono::steady_clock::now();
  for (int i=0; i<n_itt; i++)
    {
      unsigned x = rand()%n;
      unsigned z = rand()%n;

      double m = measure_pp_slow(x, z, psi);
    }
  std::chrono::steady_clock::time_point end2 = std::chrono::steady_clock::now();

  std::cout << "Measurement Time (slow) = " << std::chrono::duration_cast<std::chrono::microseconds>(end2 - begin2).count()/n_itt << "[µs]" << std::endl;
  std::cout << "Measurement Time (slow) = " << std::chrono::duration_cast<std::chrono::nanoseconds> (end2 - begin2).count()/n_itt << "[ns]" << std::endl;

}


/* intensity_partition.cpp */

#include <iostream>
#include <vector>
#include <array>

#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>

#include <pcl/point_types.h>
#include <pcl/common/transforms.h>
#include <pcl/point_cloud.h>
#include <pcl/filters/passthrough.h>
#include <pcl_ros/point_cloud.h>
#include <pcl_ros/transforms.h>

#include <boost/thread.hpp>

#include "road_recognizer/OtsuBinary.h"
#include "road_recognizer/Intensity.h"
#include "road_recognizer/Analysis.h"
#include "road_recognizer/Distribution.h"

#include <visualization_msgs/Marker.h>

typedef pcl::PointXYZI PointI;
typedef pcl::PointCloud<PointI> CloudI;
typedef pcl::PointCloud<PointI>::Ptr CloudIPtr;

class IntensityPartition
{
	private:
		bool pc_callback_flag = false;
		bool first_flag = false;

		int n_grass = 0, n_asphalt = 0;

		const int GRASS = 1;
		const int ASPHALT = 2;

		/* ---------- Tuning Params ---------- */
		const static int RANGE_RESOLUTION = 20;
		const static int THETA_RESOLUTION = 360;
		float OTSU_BINARY_INTENSITY_STD_DEVIATION_THRESHOLD = 1.0;
		float OTSU_BINARY_SEPARATION_THRESHOLD = 0.2;
		float OTSU_BINARY_SUM_OF_DIFF_FROM_AVR_THRESHOLD = 58.0;
		float RANGE_MAX = 20.0;
		bool PC_PUBLISH_FLAG = true; // true : publish grass points
		//bool PC_PUBLISH_FLAG = false; // false : publish only otsu_binary_msg
		/* ----------------------------------- */

		float dR = RANGE_MAX / (float)RANGE_RESOLUTION;
		float dTheta = 2*M_PI / (float)THETA_RESOLUTION;
		float intensity_max_all;
		float intensity_max[RANGE_RESOLUTION];
		float intensity_min[RANGE_RESOLUTION];
		float avr_grass = 0.0, avr_asphalt = 0.0;

		float s_max[RANGE_RESOLUTION];
		float s_max_var_b[RANGE_RESOLUTION];
		float otsu_threshold_tmp[RANGE_RESOLUTION];

		int pub_count = 0;
		int polar_grid_pt_cnt[RANGE_RESOLUTION][THETA_RESOLUTION];
		// float range_grid_sum_intensity[RANGE_RESOLUTION];
		float polar_grid_avr_intensity[RANGE_RESOLUTION][THETA_RESOLUTION];
		float polar_grid_sum_intensity[RANGE_RESOLUTION][THETA_RESOLUTION];
		float time_sum_otsu[RANGE_RESOLUTION];
		float time_sum_otsu_diffpow[RANGE_RESOLUTION];
		float time_mu_otsu[RANGE_RESOLUTION];
		float range_mu_otsu;
		float otsu_range_std_deviation;

		ros::NodeHandle nh;
		ros::NodeHandle local_nh;
		ros::Subscriber pc_subscriber;
		ros::Subscriber odom_subscriber;
		ros::Publisher grass_pc_publisher;
		ros::Publisher ground_pc_publisher;
		ros::Publisher otsu_binary_publisher;

		ros::Publisher marker_pub;

		sensor_msgs::PointCloud2 pub_pc;
		sensor_msgs::PointCloud2 pub_pc_tail;

		CloudIPtr input_pc_ {new CloudI};
		CloudIPtr polar_pc_ {new CloudI};

		std::vector<float> ptz_list;
		std::vector<float> ptz_list_tail;

		road_recognizer::OtsuBinary otsu_binary_msg;

		struct WB{
			float within;
			float between;
		};
		struct GA{
			float grass;
			float asphalt;
		};

		visualization_msgs::Marker line_strip;

	public:
		IntensityPartition(void);

		void visualization(void);

		void pc_callback(const sensor_msgs::PointCloud2ConstPtr&);
		void execution(void);
		void initialize(void);
		void cartesian_pt_2_polar_grid(CloudIPtr);
		//float calc_variance(std::vector<std::array<int, RANGE_RESOLUTION> >, int, int, int);
		float calc_variance(const std::vector<std::vector<int> >&, int, int, int);
		void calc_otsu_binary(void);
		void calc_diff_from_avr(void);
		void emergency_judge(void);

		CloudIPtr otsu_pc_generator(void);
		CloudIPtr otsu_pc_generator_tail(void);
};

int main(int argc, char** argv)
{
	ros::init(argc, argv, "intensity_partition");

	IntensityPartition intensity_partition;
	intensity_partition.execution();

	return 0;
}

IntensityPartition::IntensityPartition(void)
:local_nh("~")
{
	line_strip.id = 1;
	line_strip.type = visualization_msgs::Marker::LINE_STRIP;
	line_strip.scale.x = 0.01;
	line_strip.color.r = 255.0;
	line_strip.color.g = 255.0;
	line_strip.color.b = 255.0;

	otsu_binary_msg.range_division_num = RANGE_RESOLUTION;
	otsu_binary_msg.theta_division_num = THETA_RESOLUTION;
	otsu_binary_msg.range_max = RANGE_MAX;
	otsu_binary_msg.otsubinary_separation_threshold = OTSU_BINARY_SEPARATION_THRESHOLD;
	otsu_binary_msg.otsubinary_sum_of_diff_from_avr_threshold = OTSU_BINARY_SUM_OF_DIFF_FROM_AVR_THRESHOLD;
	otsu_binary_msg.intensity.resize(RANGE_RESOLUTION);
	otsu_binary_msg.analysis.resize(RANGE_RESOLUTION);

	pc_subscriber = nh.subscribe("/velodyne_clear", 10, &IntensityPartition::pc_callback, this);

	ros::Publisher marker_pub = nh.advertise<visualization_msgs::Marker>("visualization_marker", 10);

	otsu_binary_publisher = local_nh.advertise<road_recognizer::OtsuBinary>("otsu_binary_info", 10);
	grass_pc_publisher = nh.advertise<sensor_msgs::PointCloud2>("/grass_pc", 10);
	ground_pc_publisher = nh.advertise<sensor_msgs::PointCloud2>("/ground_pc", 10);
}

void IntensityPartition::visualization(void)
{
	for(float r_g = 0; r_g < RANGE_MAX; r_g += dR){
		for(float theta_g = 0; theta_g < 2 * M_PI; theta_g += dTheta){
			geometry_msgs::Point p;
			p.x = r_g * cos(theta_g);
			p.y = r_g * sin(theta_g);
			p.z = -0.01;
			line_strip.points.push_back(p);
		}
	}
	marker_pub.publish(line_strip);
}

void IntensityPartition::pc_callback(const sensor_msgs::PointCloud2ConstPtr &msg)
{
	//visualization();

	pcl::fromROSMsg(*msg, *input_pc_);
	polar_pc_ = input_pc_;

	CloudIPtr otsu_binary_pc_ {new CloudI};
	CloudIPtr otsu_binary_pc_tail_ {new CloudI};

	cartesian_pt_2_polar_grid(input_pc_);
	calc_otsu_binary();

	if(PC_PUBLISH_FLAG){
		otsu_binary_pc_ = otsu_pc_generator();
		otsu_binary_pc_tail_ = otsu_pc_generator_tail();
		pcl::toROSMsg(*otsu_binary_pc_, pub_pc);
		pcl::toROSMsg(*otsu_binary_pc_tail_, pub_pc_tail);
		grass_pc_publisher.publish(pub_pc);
		ground_pc_publisher.publish(pub_pc_tail);
		pub_count++;
		std::cout << "pub_count : " << pub_count << std::endl;
	}

	otsu_binary_publisher.publish(otsu_binary_msg);

	first_flag = true;

	ptz_list.clear();
	ptz_list_tail.clear();
}

void IntensityPartition::execution(void)
{
	ros::spin();
}

void IntensityPartition::initialize(void)
{
	otsu_binary_msg.emergency = false;
	for(int r_g = 0; r_g <RANGE_RESOLUTION; r_g++){
		for(int theta_g = 0; theta_g <THETA_RESOLUTION; theta_g++){
			polar_grid_pt_cnt[r_g][theta_g] = 0;
			polar_grid_sum_intensity[r_g][theta_g] = 0.0;
			polar_grid_avr_intensity[r_g][theta_g] = -1.0;
		}
		s_max[r_g] = 0.0;
		s_max_var_b[r_g] = 0.0;
		otsu_threshold_tmp[r_g] = 0.0;
		//range_grid_sum_intensity[r_g] = 0.0;
		time_sum_otsu[r_g] = 0.0;
		time_sum_otsu_diffpow[r_g] = 0.0;
		intensity_max[r_g] = 0.0;
		intensity_min[r_g] = 999.9;
	}
	intensity_max_all = 0.0;
}

void IntensityPartition::cartesian_pt_2_polar_grid(CloudIPtr cartesian_pc_)
{
	float r_tmp, theta_tmp, z_tmp;
	size_t i = 0;
	initialize();

	for(auto& pt : cartesian_pc_->points){
		r_tmp = sqrt(pt.x * pt.x + pt.y * pt.y);
		theta_tmp = atan2(pt.y,pt.x);
		if(theta_tmp < 0){
			theta_tmp = 2 * M_PI + theta_tmp;
		}

		// create polar grid informations
		bool r_flag = false, theta_flag = false;
		for(int r_g = 0; r_g < RANGE_RESOLUTION; r_g++){
			for(int theta_g = 0; theta_g < THETA_RESOLUTION; theta_g++){
				if((r_g == (int)(r_tmp / dR)) && (theta_g == (int)(theta_tmp / dTheta))){
					polar_grid_pt_cnt[r_g][theta_g] += 1;
					polar_grid_sum_intensity[r_g][theta_g] += pt.intensity;
					polar_grid_avr_intensity[r_g][theta_g] = polar_grid_sum_intensity[r_g][theta_g] / (float)polar_grid_pt_cnt[r_g][theta_g];

					if(PC_PUBLISH_FLAG){
						z_tmp = polar_pc_->points[i].z;
						ptz_list.push_back(z_tmp);
						ptz_list_tail.push_back(z_tmp);
					}
					polar_pc_->points[i].z = r_tmp;

					r_flag = true;
					theta_flag = true;
				}

				if(intensity_min[r_g] > polar_grid_avr_intensity[r_g][theta_g] || !first_flag){
					intensity_min[r_g] = polar_grid_avr_intensity[r_g][theta_g];
				}
				if(intensity_max[r_g] < polar_grid_avr_intensity[r_g][theta_g] || !first_flag){
					intensity_max[r_g] = polar_grid_avr_intensity[r_g][theta_g];
				}

				if(theta_flag) break;
			}

		}

		if(intensity_max_all < pt.intensity){
			intensity_max_all = pt.intensity;
		}

		i++;
	}

	for(int r_g = 0; r_g < RANGE_RESOLUTION; r_g++){
		if(r_g == 0){
			otsu_binary_msg.intensity[r_g].min = 0.0;
			otsu_binary_msg.intensity[r_g].max = 0.0;
		}else{
			otsu_binary_msg.intensity[r_g].min = intensity_min[r_g];
			otsu_binary_msg.intensity[r_g].max = intensity_max[r_g];
		}
		/* for(int theta_g = 0; theta_g < THETA_RESOLUTION; theta_g++){ */
		/* 	range_grid_sum_intensity[r_g] += polar_grid_avr_intensity[r_g][theta_g]; */
		/* } */
	}
}

//float IntensityPartition::calc_variance(std::vector<std::array<int, RANGE_RESOLUTION> > histogram_list, int r_g, int threshold_tmp, int grass_or_asphalt)
float IntensityPartition::calc_variance(const std::vector<std::vector<int> >& histogram_list, int r_g, int threshold_tmp, int grass_or_asphalt)
{
	float variance = 0, sum = 0, multi_sum = 0, diffpow_sum = 0, mu = 0;
	int cnt = 0;

	switch(grass_or_asphalt){
		case 1: // grass
			for(int i = 0; i < threshold_tmp; i++){
				multi_sum += (float)histogram_list[i][r_g] * i;
				sum += (float)histogram_list[i][r_g];
				cnt++;
			}
			n_grass = cnt;
			mu = multi_sum / sum;
			avr_grass = mu;
			for(int i = 0; i < threshold_tmp; i++){
				float diff = (float)histogram_list[i][r_g] - mu;
				diffpow_sum += diff * diff;
			}
			variance = diffpow_sum / cnt;
			break;

		case 2: //asphalt
			for(int i = threshold_tmp; i < (int)intensity_max[r_g]; i++){
				multi_sum += (float)histogram_list[i][r_g] * i;
				sum += (float)histogram_list[i][r_g];
				cnt++;
			}
			n_asphalt = cnt;
			mu = multi_sum / sum;
			avr_asphalt = mu;
			for(int i = threshold_tmp; i < (int)intensity_max[r_g]; i++){
				float diff = (float)histogram_list[i][r_g] - mu;
				diffpow_sum += diff * diff;
			}
			variance = diffpow_sum / cnt;
			break;
	}

	return variance;
}

void IntensityPartition::calc_diff_from_avr(void)
{
	float range_sum_otsu = 0.0;
	for(int r_g = 1; r_g < RANGE_RESOLUTION; r_g++){
		range_sum_otsu += otsu_binary_msg.intensity[r_g].threshold;
	}
	float range_mu_otsu = range_sum_otsu / (RANGE_RESOLUTION - 1);

	for(int r_g = 0; r_g < RANGE_RESOLUTION; r_g++){
		float diff = otsu_binary_msg.intensity[r_g].threshold - range_mu_otsu;
		otsu_binary_msg.analysis[r_g].otsubinary_diff_from_thresholds_avr = sqrt(diff * diff);
	}
}

void IntensityPartition::emergency_judge(void)
{
	float sum_diff_from_avr = 0.0;
	for(int r_g = 0; r_g < RANGE_RESOLUTION; r_g++){
		sum_diff_from_avr += otsu_binary_msg.analysis[r_g].otsubinary_diff_from_thresholds_avr;
	}
	if(sum_diff_from_avr > OTSU_BINARY_SUM_OF_DIFF_FROM_AVR_THRESHOLD){
		otsu_binary_msg.emergency = true;
	}
}

void IntensityPartition::calc_otsu_binary(void)
{
	//const int histogram_size = (int)intensity_max_all;
	const int histogram_size = 256;
	float sum_all[RANGE_RESOLUTION], avr_all[RANGE_RESOLUTION], var_all[RANGE_RESOLUTION], diff_all[histogram_size],
		  sum_tmp1[RANGE_RESOLUTION], sum_tmp2[RANGE_RESOLUTION],
		  multi_sum_all[RANGE_RESOLUTION];
	/* std::array<int, RANGE_RESOLUTION> r_res_array; */
	std::vector<int> r_res_array;
	//std::vector<std::array<int, RANGE_RESOLUTION> > histogram;
	std::vector<std::vector<int> > histogram;

	// initialize
	for(int r_g = 0; r_g < RANGE_RESOLUTION; r_g++){
		r_res_array.push_back((int)0);
		multi_sum_all[r_g] = 0;
		sum_all[r_g] = 0;
		sum_tmp1[r_g] = 0;
		sum_tmp2[r_g] = 0;
	}
	for(int i = 0; i < histogram_size; i++){
		histogram.push_back(r_res_array);
	}

	// make histogram
	for(int r_g = 0; r_g < RANGE_RESOLUTION; r_g++){
		for(int theta_g = 0; theta_g < THETA_RESOLUTION; theta_g++){
			int intensity_tmp = (int)polar_grid_avr_intensity[r_g][theta_g];
			if(intensity_tmp > 0){
				histogram.at(intensity_tmp).at(r_g) += 1;
			}
		}
	}

	// for debag
	for(int r_g=0; r_g < RANGE_RESOLUTION; r_g++){
		otsu_binary_msg.analysis[r_g].distribution.resize(histogram_size);
		for(int idx_intensity = 0; idx_intensity < histogram_size; idx_intensity++){
			otsu_binary_msg.analysis[r_g].distribution[idx_intensity].intensity = (float)histogram[idx_intensity][r_g];
		}
	}

	// calc separation
	struct GA var[RANGE_RESOLUTION][histogram_size-1];
	struct GA num[RANGE_RESOLUTION][histogram_size-1];
	struct GA avr[RANGE_RESOLUTION][histogram_size-1];
	for(int r_g = 0; r_g < RANGE_RESOLUTION; r_g++){
		for(int i_threshold = 1; i_threshold < (int)intensity_max[r_g]; i_threshold++){
			var[r_g][i_threshold-1].grass = calc_variance(histogram, r_g, i_threshold, GRASS);
			var[r_g][i_threshold-1].asphalt = calc_variance(histogram, r_g, i_threshold, ASPHALT);
			num[r_g][i_threshold-1].grass = (float)n_grass;
			num[r_g][i_threshold-1].asphalt = (float)n_asphalt;
			avr[r_g][i_threshold-1].grass = avr_grass;
			avr[r_g][i_threshold-1].asphalt = avr_asphalt;
		}
	}

	// calc whole variance
	for(int r_g = 0; r_g < RANGE_RESOLUTION; r_g++){
		for(int idx_intensity = 0; idx_intensity < (int)intensity_max[r_g]; idx_intensity++){
			multi_sum_all[r_g] += histogram[idx_intensity][r_g] * idx_intensity;
			sum_all[r_g] += histogram[idx_intensity][r_g];
		}
		avr_all[r_g] = (float)multi_sum_all[r_g] / (float)sum_all[r_g];
		for(int i_threshold = 0; i_threshold < (int)intensity_max[r_g]; i_threshold++){
			diff_all[i_threshold] = (float)histogram[i_threshold][r_g] - avr_all[r_g];
			sum_tmp1[r_g] += diff_all[i_threshold] * diff_all[i_threshold];
		}
		var_all[r_g] = sum_tmp1[r_g] / (float)(sum_all[r_g]);
		/* std::cout << "var_all[" << r_g << "] : " << var_all[r_g] << std::endl; */
		otsu_binary_msg.analysis[r_g].intensity_std_deviation = sqrt(var_all[r_g]);
		/* std::cout << "intensity_std_deviation[" << r_g << "] : " << otsu_binary_msg.analysis[r_g].intensity_std_deviation << std::endl; */
	}

	struct WB var_wb;

	/* std::cout << "==========" << std::endl; */
	for(int r_g = 0; r_g < RANGE_RESOLUTION; r_g++){
		for(int i_threshold = 1; i_threshold < (int)intensity_max[r_g]; i_threshold++){
			float ng = (float)num[r_g][i_threshold-1].grass;
			float na = (float)num[r_g][i_threshold-1].asphalt;
			var_wb.within = (ng * var[r_g][i_threshold-1].grass + na * var[r_g][i_threshold-1].asphalt) / (ng + na);
			float diff_mg = avr[r_g][i_threshold-1].grass - avr_all[r_g];
			float diff_ma = avr[r_g][i_threshold-1].asphalt - avr_all[r_g];
			var_wb.between = (ng * diff_mg * diff_mg + na * diff_ma * diff_ma) / (ng + na);
			//var_wb.between = ng * na * (avr[r_g][i_threshold-1].grass - avr[r_g][i_threshold-1].asphalt) * (avr[r_g][i_threshold-1].grass - avr[r_g][i_threshold-1].asphalt) / (ng + na) * (ng + na);
			float s_tmp = var_wb.between / var_wb.within;

			if(s_max[r_g] < s_tmp){
				s_max[r_g] = s_tmp;
				s_max_var_b[r_g] = var[r_g][i_threshold-1].asphalt;
				otsu_threshold_tmp[r_g] = (float)i_threshold;
			}
		}

		if(r_g == 0){
			otsu_binary_msg.intensity[r_g].threshold = 0.0;
			otsu_binary_msg.analysis[r_g].separation = 0.0;
		}else{
			otsu_binary_msg.intensity[r_g].threshold = otsu_threshold_tmp[r_g];
			otsu_binary_msg.analysis[r_g].separation = s_max[r_g];
		}
		/* std::cout << "----------" << std::endl; */
		/* std::cout << "r = " << r_g << ", threshold = " << otsu_binary_msg.intensity[r_g].threshold << std::endl; */
		/* std::cout << "r = " << r_g << ", S = " << otsu_binary_msg.analysis[r_g].separation << std::endl; */
		//std::cout << "r = " << r_g << ", intensity_max = " << intensity_max[r_g] << std::endl;

		for(int i = 0; i < intensity_max[r_g]; i++){
			if(otsu_binary_msg.analysis[r_g].distribution[i].intensity > 0){
				//std::cout << "r = " << r_g << ", histogram[" << i << "] = " << otsu_binary_msg.analysis[r_g].distribution[i].intensity << std::endl;
			}
		}
		//std::cout << "----------" << std::endl;
	}

	//std::cout << "==========" << std::endl;

	// calc trust range

	// for debag

	// calc skewness
	for(int r_g = 0; r_g < RANGE_RESOLUTION; r_g++){
		for(int idx_intensity = 1; idx_intensity < (int)intensity_max[r_g]; idx_intensity++){
			float z_score = diff_all[idx_intensity-1] / var_all[r_g];
			sum_tmp2[r_g] += z_score * z_score * z_score;
		}
		/* std::cout << "sum_tmp2[" << r_g << "] : " << sum_tmp2[r_g] << std::endl; */
		/* std::cout << "n_all[" << r_g << "] : " << n_all[r_g] << std::endl; */
		/* if(r_g == 0){ */
		/* 	otsu_binary_msg.analysis[r_g].skewness = 0.0; */
		/* }else{ */
		otsu_binary_msg.analysis[r_g].skewness = sum_tmp2[r_g] / (float)sum_all[r_g];
		/* std::cout << "otsu_binary_msg.analysis[" << r_g << "].skewness : " << otsu_binary_msg.analysis[r_g].skewness << std::endl; */
		// }
	}

	// calc threshold histogram in range
	calc_diff_from_avr();

	// judge emergency
	emergency_judge();

	r_res_array.clear();
	histogram.clear();
}

CloudIPtr IntensityPartition::otsu_pc_generator(void)
{
	size_t iz = 0;
	CloudIPtr p_ {new CloudI};
	p_ = polar_pc_;
	for(auto& pt : p_->points){
		int RG = sqrt(pt.x * pt.x + pt.y * pt.y) / dR;
		int thetag = atan2(pt.y,pt.x);
		if(thetag < 0){
			thetag = 2 * M_PI + thetag;
		}
		int THETAG = thetag / dTheta;

		for(int r_g = 0; r_g < RANGE_RESOLUTION; r_g++){
			//for(int theta_g = 0; theta_g < THETA_RESOLUTION; theta_g++){
				//if(((float)r_g <= pt.z && pt.z < (float)r_g+dR) && ((float)theta_g <= thetag && thetag < (float)theta_g + dTheta)
				if(((float)r_g <= pt.z && pt.z < (float)r_g+dR)
					/* && ((otsu_threshold_tmp[r_g] - 1.0 > polar_grid_avr_intensity[RG][THETAG]) */
					&& ((otsu_threshold_tmp[r_g] > pt.intensity)
						//|| (otsu_binary_msg.analysis[r_g].intensity_std_deviation <  OTSU_BINARY_INTENSITY_STD_DEVIATION_THRESHOLD)
						|| (otsu_binary_msg.analysis[r_g].separation < OTSU_BINARY_SEPARATION_THRESHOLD)
						)){
					pt.intensity = -1.0;
				}
			//}
		}
		pt.z = ptz_list.at(iz);
		iz++;
	}

	if(otsu_binary_msg.emergency){
		//intensity_max_all = 0.0;
	}

	pcl::PassThrough<PointI> pass;
	CloudIPtr filtered_pc_ {new CloudI};
	pass.setInputCloud(polar_pc_);
	pass.setFilterFieldName ("intensity");
	pass.setFilterLimits(0.0, intensity_max_all);
	//pass.setFilterLimitsNegative (true);
	pass.filter(*filtered_pc_);

	size_t otsu_size = filtered_pc_->points.size();

	CloudIPtr otsu_pc_ {new CloudI};
	otsu_pc_->points.resize(otsu_size);
	otsu_pc_->header = input_pc_->header;
	size_t i = 0;
	for(auto& pt : otsu_pc_->points){
		pt.x = filtered_pc_->points[i].x;
		pt.y = filtered_pc_->points[i].y;
		pt.z = filtered_pc_->points[i].z;
		pt.intensity = filtered_pc_->points[i].intensity;
		i++;
	}

	return otsu_pc_;
}

CloudIPtr IntensityPartition::otsu_pc_generator_tail(void)
{
	size_t iz = 0;
	CloudIPtr p_ {new CloudI};
	p_ = polar_pc_;
	for(auto& pt : p_->points){
		int RG = sqrt(pt.x * pt.x + pt.y * pt.y) / dR;
		int thetag = atan2(pt.y,pt.x);
		if(thetag < 0){
			thetag = 2 * M_PI + thetag;
		}
		int THETAG = thetag / dTheta;

		for(int r_g = 0; r_g < RANGE_RESOLUTION; r_g++){
			//for(int theta_g = 0; theta_g < THETA_RESOLUTION; theta_g++){
				//if(((float)r_g <= pt.z && pt.z < (float)r_g+dR) && ((float)theta_g <= thetag && thetag < (float)theta_g + dTheta)
				if(((float)r_g <= pt.z && pt.z < (float)r_g+dR)
					/* && ((otsu_threshold_tmp[r_g] - 1.0 > polar_grid_avr_intensity[RG][THETAG]) */
					&& ((otsu_threshold_tmp[r_g] > pt.intensity)
						//|| (otsu_binary_msg.analysis[r_g].intensity_std_deviation <  OTSU_BINARY_INTENSITY_STD_DEVIATION_THRESHOLD)
						|| (otsu_binary_msg.analysis[r_g].separation < OTSU_BINARY_SEPARATION_THRESHOLD)
						)){
					pt.intensity = -1.0;
				}
			//}
		}
		pt.z = ptz_list.at(iz);
		/* if(pt.z > 0.5){ */
		/* 	pt.intensity = -1.0; */
		/* } */
		iz++;
	}

	if(otsu_binary_msg.emergency){
		//intensity_max_all = 0.0;
	}

	pcl::PassThrough<PointI> pass;
	CloudIPtr filtered_pc_ {new CloudI};
	pass.setInputCloud(polar_pc_);
	pass.setFilterFieldName ("intensity");
	pass.setFilterLimits(0.0, intensity_max_all);
	pass.setFilterLimitsNegative (true);
	pass.filter(*filtered_pc_);

	size_t otsu_size = filtered_pc_->points.size();

	CloudIPtr otsu_pc_ {new CloudI};
	otsu_pc_->points.resize(otsu_size);
	otsu_pc_->header = input_pc_->header;
	size_t i = 0;
	for(auto& pt : otsu_pc_->points){
		pt.x = filtered_pc_->points[i].x;
		pt.y = filtered_pc_->points[i].y;
		pt.z = filtered_pc_->points[i].z;
		pt.intensity = filtered_pc_->points[i].intensity;
		i++;
	}

	return otsu_pc_;
}


// Copyright University of Warwick 2014
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

// Authors:
// John Back

/*! \file LpcPathLength.hh
    \brief File containing the declaration of the LpcPathLength class
*/

/*! \class LpcPathLength
    \brief Class used to define a series of pathlengths along the lpc
*/

#ifndef LPC_PATH_LENGTH_HH
#define LPC_PATH_LENGTH_HH

#include <Eigen/Dense>

class LpcPathLength {

public:

    //! Default, empty constructor
    LpcPathLength();

    //! Constructor using the results of the scaled pathlengths found by LpcFollow
    /*!
      \param [in] sLambda the scaled cumulative 3d path length
      \param [in] sLambdaAxes the scaled cumulative path length along each co-ordinate axis
      \param [in] theRange the scaling range for x,y,z
    */
    LpcPathLength(const Eigen::VectorXd& sLambda,
		  const Eigen::MatrixXd& sLambdaAxes,
		  const Eigen::VectorXd& theRange);

    //! Copy constructor
    /*!
      \param [in] other The LpcPathLength to copy
    */
    LpcPathLength(const LpcPathLength& other);

    //! Assignment operator using another LpcPathLength
    /*!
      \param [in] other The LpcPathLength used in the assignment
    */
    LpcPathLength& operator=(const LpcPathLength& other);

    //! Destructor
    virtual ~LpcPathLength();

    // Accessors.

    //! Get the number of pathlength segments
    /*!
      \return the number of pathlength segments
    */
    int getN() const {return N_;}

    //! Get the scaled cumulative 3d path length as an Eigen VectorXd
    /*!
      \returns the scaled cumulative 3d path length
    */
    Eigen::VectorXd getScaledLambda() const {return sL_;}

    //! Get the scaled cumulative path length along x as an Eigen VectorXd
    /*!
      \returns the scaled cumulative path length along x
    */
    Eigen::MatrixXd getScaledLambdaAxes() const {return sLAxes_;}

    //! Get the scaling co-ordinate range
    /*!
      \returns the scaling co-ordinate range as an Eigen 3-vector
    */
    Eigen::VectorXd getRange() const {return theRange_;}

    //! Get the ordered (start = 0) scaled cumulative 3d path length as an Eigen VectorXd
    /*!
      \returns the ordered scaled cumulative 3d path length
    */
    Eigen::VectorXd getScaledDeltaLambda() const {return sdL_;}

    //! Get the unscaled cumulative 3d path length as an Eigen VectorXd
    /*!
      \returns the cumulative 3d path length
    */
    Eigen::VectorXd getLambda() const {return L_;}

    //! Get the unscaled cumulative path length along each axis (row = point, col = x,y,z,...)
    /*!
      \returns the unscaled cumulative path length along each axis as an Eigen MatrixXd
    */
    Eigen::MatrixXd getLambdaAxes() const {return LAxes_;}

    //! Get the unscaled ordered (start = 0) cumulative path length as an Eigen VectorXd
    /*!
      \returns the ordered unscaled cumulative path length
    */
    Eigen::VectorXd getDeltaLambda() const {return dL_;}

protected:

private:

    //! The number of pathlength segments
    int N_;

    //! The number of co-ordinate dimensions
    int nDim_;

    //! The values of the scaled cumulative path length
    Eigen::VectorXd sL_;

    //! The values of the scaled cumulative path length along each co-ordinate axis
    Eigen::MatrixXd sLAxes_;

    //! The values of the co-ordinate scaling range
    Eigen::VectorXd theRange_;

    //! The values of the scaled ordered cumulative path length, starting at 0
    Eigen::VectorXd sdL_;

    //! The values of the unscaled cumulative path length
    Eigen::VectorXd L_;

    //! The values of the unscaled cumulative path length along each co-ordinate axis
    Eigen::MatrixXd LAxes_;

    //! The values of the unscaled ordered cumulative path length, starting at 0
    Eigen::VectorXd dL_;

    //! Initialise function
    void initialise();

    //! Helper function to find the path length difference between lpc points
    /*!
      \param [in] i the index of the first lpc point
      \param [in] j the index of the second (neighbouring) lpc point
      \returns an Eigen vector of the x,y,z,... path length between the points
    */
    Eigen::VectorXd getDiff(int i, int j);

};

#endif


#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/common/transforms.h>
#include <string>
#include <pcl/io/boost.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/extract_indices.h>
#include <Eigen/Dense>
#include <stdio.h>
#include <stdlib.h>
#include <time.h>

#ifndef CUBE_ALIGNER_H
#define CUBE_ALIGNER_H

#define RAND_SCALE 100000
#define V_CT 7

#ifdef ADAPT_SIMPLEX
  #define ALPHA 1.0
  #define BETA (4.0/3.0)
  #define GAMMA (2.0/3.0)
  #define DELTA (5.0/6.0)
#else
  #define ALPHA 1.0
  #define BETA 2.0
  #define GAMMA 0.5
  #define DELTA 0.5
#endif

#define CUBE_MAX_ITER 500
//#define E_VAL 0.1
#define EPS_VAL 1200
#define MIN_CLUSTER_DENS 0.66

#ifndef MY_PI
#define MY_PI 3.141592
#endif

using namespace std;
using namespace pcl;

static bool isRandSeeded = false;

static double E_VAL = 0.2;

class CVertex
{
public:
  CVertex(){
    _alpha = 0;
    _beta = 0;
    _gamma = 0;
    _x = 0;
    _y = 0;
    _z = 0;
  }

  CVertex(double a, double b, double g, double x, double y, double z)
  {
    _alpha = a;
    _beta = b;
    _gamma = g;
    _x = x;
    _y = y;
    _z = z;
    _score = 0;
  }

  CVertex(Eigen::Matrix4f input)
  {
    double cb;
    _x = input(0,3);
    _y = input(1,3);
    _z = input(2,3);
    _beta = asin(-input(2,0));
    cb = cos(_beta);
    _alpha = asin(input(1,0)/cb);
    _gamma = asin(input(2,1)/cb);

    _score = 0;
  }

  /*CVertex(Eigen::Matrix4d input)
  {
    double cb;
    _x = input(0,3);
    _y = input(1,3);
    _z = input(2,3);
    _beta = asin(-input(2,0));
    cb = cos(_beta);
    _alpha = asin(input(1,0)/cb);
    _gamma = asin(input(2,1)/cb);

    _score = 0;
  }*/

#define RAD_SCALE 1.0

  CVertex(int i, double d, const CVertex& vert)
  {
    if (!isRandSeeded)
    {
      srand(time(NULL));
      isRandSeeded = true;
    }
    int m = i/7;
    _alpha = vert._alpha;
    _beta = vert._beta;
    _gamma = vert._gamma;
    _x = vert._x;
    _y = vert._y;
    _z = vert._z;

    switch(i%7)
    {
      case 0:
	_alpha += (1-2*m)*d/RAD_SCALE;
	break;
      case 1:
	_beta += (1-2*m)*d/RAD_SCALE;
	break;
      case 2:
	_gamma += (1-2*m)*d/RAD_SCALE;
	break;
      case 3:
	_x += (1-2*m)*d;
	break;
      case 4:
	_y += (1-2*m)*d;
	break;
      case 5:
	_z += (1-2*m)*d;
	break;
      default:
	break;
    }
    /*switch(j)
    {
      case 1:
	_alpha += d/10.0;
	break;
      case 2:
	_beta += d/10.0;
	break;
      case 3:
	_gamma += d/10.0;
	break;
      case 4:
	_x += d/10.0;
	break;
      case 5:
	_y += d/10.0;
	break;
      case 6:
	_z += d/10.0;
	break;
      default:
	break;
    }*/
    _score = 0;
  }

  CVertex(const CVertex& cg)
  {
    _alpha = cg._alpha;
    _beta = cg._beta;
    _gamma = cg._gamma;
    _x = cg._x;
    _y = cg._y;
    _z = cg._z;
    _score = 0;
  }

  CVertex(CVertex *cgArr, int count)
  {
    int i;
    for (i=0;i<count;i++)
    {
      _alpha = cgArr[i]._alpha/count;
      _beta = cgArr[i]._beta/count;
      _gamma = cgArr[i]._gamma/count;
      _x = cgArr[i]._x/count;
      _y = cgArr[i]._y/count;
      _z = cgArr[i]._z/count;
    }
    _score = 0;
  }

  CVertex(double maxRot, double maxTran)
  {
    if (!isRandSeeded)
    {
      srand(time(NULL));
      isRandSeeded = true;
    }
    _alpha = (((rand()% (RAND_SCALE*2))-RAND_SCALE)*maxRot)/(double)RAND_SCALE;
    _beta = (((rand()% (RAND_SCALE*2))-RAND_SCALE)*maxRot)/(double)RAND_SCALE;
    _gamma = (((rand()% (RAND_SCALE*2))-RAND_SCALE)*maxRot)/(double)RAND_SCALE;
    _x = (((rand()% (RAND_SCALE*2))-RAND_SCALE)*maxTran)/(double)RAND_SCALE;
    _y = (((rand()% (RAND_SCALE*2))-RAND_SCALE)*maxTran)/(double)RAND_SCALE;
    _z = (((rand()% (RAND_SCALE*2))-RAND_SCALE)*maxTran)/(double)RAND_SCALE;
    _score = 0;
  }

  inline double getDist()
  {
    return sqrt(_x*_x + _y*_y + _z*_z);
  }

  inline double getMaxDiff(CVertex comp)
  {
    double diff = 0;

    if (fabs(_x-comp._x) > diff)
    {
      diff = fabs(_x-comp._x);
    }
    if (fabs(_y-comp._y) > diff)
    {
      diff = fabs(_y-comp._y);
    }
    if (fabs(_z-comp._z) > diff)
    {
      diff = fabs(_z-comp._z);
    }
    if (fabs(_alpha-comp._alpha) > diff)
    {
      diff = fabs(_alpha-comp._alpha);
    }
    if (fabs(_beta-comp._beta) > diff)
    {
      diff = fabs(_beta-comp._beta);
    }
    if (fabs(_gamma-comp._gamma) > diff)
    {
      diff = fabs(_gamma-comp._gamma);
    }

    return diff;
  }

  inline Eigen::Matrix4f toTranslation()
  {
    Eigen::Matrix4f outMat;
    double ca = cos(_alpha);
    double cb = cos(_beta);
    double cg = cos(_gamma);
    double sa = sin(_alpha);
    double sb = sin(_beta);
    double sg = sin(_gamma);

    //build the trans-rot matrix
    outMat(0,0) = ca*cb;
    outMat(0,1) = ca*sb*sg - sa*cg;
    outMat(0,2) = sa*sg - ca*sb*cg;
    outMat(1,0) = sa*cb;
    outMat(1,1) = ca*cg - sa*sb*sg;
    outMat(1,2) = sa*sb*cg - ca*sg;
    outMat(2,0) = -sb;
    outMat(2,1) = cb*sg;
    outMat(2,2) = cb*cg;
    outMat(3,0) = 0;outMat(3,1)=0;outMat(3,2)=0;
    outMat(0,3) = _x;
    outMat(1,3) = _y;
    outMat(2,3) = _z;
    outMat(3,3) = 1;

    return outMat;
  }

  inline void setScore(int score)
  {
    _score = score;
  }

  inline int getScore()
  {
    return _score;
  }

  inline static int sorter(const void *v1, const void *v2)
  {
    CVertex *cv1,*cv2;
    if (!v1 || !v2)
    {
      return 0;
    }
    cv1 = (CVertex*)v1;
    cv2 = (CVertex*)v2;

    return (cv1->_score < cv2->_score) -
      2*(cv1->_score > cv2->_score);
  }

  inline CVertex operator+(const CVertex& vIn)
  {
    CVertex outVert(this,1);
    outVert += vIn;

    return outVert;
  }

  inline CVertex operator+=(const CVertex& vIn)
  {
    _alpha += vIn._alpha;
    _beta += vIn._beta;
    _gamma += vIn._gamma;
    _x += vIn._x;
    _y += vIn._y;
    _z += vIn._z;

    return *this;
  }

  inline CVertex operator-(const CVertex& vIn)
  {
    CVertex outVert(this,1);
    outVert -= vIn;

    return outVert;
  }

  inline CVertex operator-=(const CVertex& vIn)
  {
    _alpha -= vIn._alpha;
    _beta -= vIn._beta;
    _gamma -= vIn._gamma;
    _x -= vIn._x;
    _y -= vIn._y;
    _z -= vIn._z;

    return *this;
  }

  inline CVertex operator*(float alpha)
  {
    CVertex outVert(this,1);
    outVert *= alpha;

    return outVert;
  }

  inline CVertex operator*=(float alpha)
  {
    _alpha *= alpha;
    _beta *= alpha;
    _gamma *= alpha;
    _x *= alpha;
    _y *= alpha;
    _z *= alpha;

    return *this;
  }

  inline double getX(){return _x;}
  inline double getY(){return _y;}
  inline double getZ(){return _z;}

private:
  friend std::ostream& operator<<(std::ostream &strm, const CVertex& vert);
  double _alpha;
  double _beta;
  double _gamma;
  double _x;
  double _y;
  double _z;
  int _score;
};

//add the scalar*matrix operator outside the class
inline CVertex operator*(float alpha, const CVertex& vert)
{
  CVertex outMat(vert);
  outMat *= alpha;
  return outMat;
}

//String writer
std::ostream& operator<<(std::ostream &strm, const CVertex& vert)
{
  return strm << "(" << vert._x << "," << vert._y << "," << vert._z << "," << vert._alpha << "," << vert._beta << "," << vert._gamma <<")";
}

class CubeAligner
{
public:
  typedef PointCloud<PointXYZI> Cloud;
  typedef PointCloud<PointXYZI>::Ptr CloudPtr;
  typedef PointCloud<PointXYZI>::iterator CloudItr;

  CubeAligner(CloudPtr cloudBase, double e)
  {
    CloudItr npIt = cloudBase->begin();
    _min_x=0;
    _max_x=0;
    _min_y=0;
    _max_y=0;
    _min_z=0;
    _max_z=0;
    _arrSz = 0;

    while (npIt<cloudBase->end())
    {
      if (_min_x==0)
      {
	_min_x = (*npIt).x;
	_max_x = (*npIt).x;
	_min_y = (*npIt).y;
	_max_y = (*npIt).y;
	_min_z = (*npIt).z;
	_max_z = (*npIt).z;
      }
      else
      {
	if ((*npIt).x < _min_x)
	{
	  _min_x = (*npIt).x;
	}
	if ((*npIt).x > _max_x)
	{
	  _max_x = (*npIt).x;
	}
	if ((*npIt).y < _min_y)
	{
	  _min_y = (*npIt).y;
	}
	if ((*npIt).y > _max_y)
	{
	  _max_y = (*npIt).y;
	}
	if ((*npIt).z < _min_z)
	{
	  _min_z = (*npIt).z;
	}
	if ((*npIt).z > _max_z)
	{
	  _max_z = (*npIt).z;
	}
      }
      npIt++;
    }
    int gf=0;

    do
    {
      _e = e + ((gf++) * 0.01);
      _xCount = (_max_x - _min_x)/_e;
      _yCount = (_max_y - _min_y)/_e;
      _zCount = (_max_z - _min_z)/_e;
    }
    while (_xCount*_yCount*_zCount <= 0);

    //printf("Setup cube grid for (%.2f,%.2f,%.2f)->(%.2f,%.2f,%.2f)\n"
    //"%ld grid points\n",
	   //_min_x,_min_y,_min_z,_max_x,_max_y,_max_z,arraySizeNeeded());
    _vBase = (unsigned char *)malloc(arraySizeNeeded());
    memset(_vBase,0,arraySizeNeeded());

    //printf("%d/%ld filled grid points\n",cloudToArray(cloudBase,_vBase),_xCount*_yCount*_zCount);
    cloudToArray(cloudBase,_vBase);
  }

  ~CubeAligner()
  {
    if (_vBase)
    {
      free(_vBase);
    }
  }

  inline void idxToXYZ(long idx, double *x, double *y, double *z)
  {
    int ix,iy,iz;
    ix = idx%_xCount;
    iy = (idx/_xCount)%_yCount;
    iz = idx/_xCount/_yCount;

    *x = _min_x + ix*_e;
    *y = _min_y + iy*_e;
    *z = _min_z + iz*_e;

    return;
  }

  inline long toIdx(long ix, long iy, long iz)
  {
    if (ix < 0 || ix >= _xCount ||
      iy < 0 || iy >= _yCount ||
      iz < 0 || iz >= _zCount)
      return -1;

    return ix+iy*_xCount+iz*_xCount*_yCount;
  }

  inline size_t arraySizeNeeded()
  {
    size_t totalCount;

    if (!_arrSz)
    {
      totalCount = _xCount*_yCount*_zCount;

      totalCount += (totalCount%8)?8:0;

      _arrSz = totalCount/8;
    }

    return _arrSz;
  }

  inline int cloudToArray(CloudPtr cloud, unsigned char *v)
  {
    CloudItr npIt = cloud->begin();
    long ix,iy,iz;
    int bitIdx;
    long long idx;
    size_t arrIdx;
    int ptCount = 0;
    int matchCount = 0;

    while (npIt<cloud->end())
    {
      ptCount++;
      ix = ((*npIt).x-_min_x)/_e;
      iy = ((*npIt).y-_min_y)/_e;
      iz = ((*npIt).z-_min_z)/_e;
      //ignore points outsize range
      if (ix < 0 || iy < 0 || iz < 0
	|| ix >= _xCount || iy >= _yCount || iz >= _zCount)
      {npIt++;continue;}

      matchCount++;

      idx = ix+iy*_xCount+iz*_xCount*_yCount;
      arrIdx = idx/8;
      bitIdx = idx%8;
      v[arrIdx] |= (0x80 >> bitIdx);

      npIt++;
    }

    return matchCount;
  }

  class CCluster
  {
  public:
    CCluster(CubeAligner *cube){_my_cube = cube;}

    inline void addIdx(long idx)
    {
      _my_pts.push_back(idx);
    }

    inline void addIdx(long ix, long iy, long iz)
    {
      _my_pts.push_back(_my_cube->toIdx(ix,iy,iz));
    }

    inline void calcDimensions()
    {
      std::vector<long>::iterator it;
      double x,y,z;
      double mX=0,mY=0,mZ=0; // max values
      //set origin to obsurdly large value to fin min value
      _x_orig = 1e9;
      _y_orig = 1e9;
      _z_orig = 1e9;

      for (it = _my_pts.begin();it < _my_pts.end(); it++)
      {
	_my_cube->idxToXYZ(*it,&x,&y,&z);

	if (x < _x_orig){_x_orig = x;}
	if (y < _y_orig){_y_orig = y;}
	if (z < _z_orig){_z_orig = z;}
	if (x > mX){mX = x;}
	if (y > mY){mY = y;}
	if (z > mZ){mZ = z;}
      }
      _dx = mX - _x_orig;
      _dy = mY - _y_orig;
      _dz = mZ - _z_orig;

      return;
    }

    inline bool isInside(long idx)
    {
      double x,y,z;
      _my_cube->idxToXYZ(idx,&x,&y,&z);

      if (x >= _x_orig && x <= (_x_orig+_dx) &&
	y >= _y_orig && y <= (_y_orig+_dy) &&
	z >= _z_orig && z <= (_z_orig+_dz))
      {
	return true;
      }
      else
      {
	return false;
      }
    }

    inline bool includes(long idx)
    {
      std::vector<long>::iterator it;

      for (it = _my_pts.begin(); it < _my_pts.end(); it++)
      {
	if (*it == idx)
	{
	  return true;
	}
      }

      return false;
    }

    inline PointXYZI getCOG()
    {
      std::vector<long>::iterator lIt;
      PointXYZI p1;
      double x,y,z;
      p1.x = 0;
      p1.y = 0;
      p1.z = 0;

      for (lIt = _my_pts.begin(); lIt < _my_pts.end() ; lIt++)
      {
	_my_cube->idxToXYZ(*lIt,&x,&y,&z);
	p1.x += x/_my_pts.size();
	p1.y += y/_my_pts.size();
	p1.z += z/_my_pts.size();
      }

      return p1;
    }

    inline CloudPtr toPolygon(int i)
    {
      CloudPtr cloudOut(new Cloud);
      std::vector<long>::iterator lIt;

      PointXYZI p1;
      double x,y,z;

      for (lIt = _my_pts.begin(); lIt < _my_pts.end() ; lIt++)
      {
	_my_cube->idxToXYZ(*lIt,&x,&y,&z);
	p1.x = x;
	p1.y = y;
	p1.z = z;
  p1.intensity = i;
	cloudOut->push_back(p1);
      }

//       PointXYZI p1,p2,p3,p4,p5,p6,p7,p8;
//       p1.x = _x_orig;
//       p1.y = _y_orig;
//       p1.z = _z_orig;
//       p2.x = _x_orig+_dx;
//       p2.y = _y_orig;
//       p2.z = _z_orig;
//       p3.x = _x_orig+_dx;
//       p3.y = _y_orig+_dy;
//       p3.z = _z_orig;
//       p4.x = _x_orig;
//       p4.y = _y_orig+_dy;
//       p4.z = _z_orig;
//       p5.x = _x_orig;
//       p5.y = _y_orig;
//       p5.z = _z_orig + _dz;
//       p6.x = _x_orig+_dx;
//       p6.y = _y_orig;
//       p6.z = _z_orig + _dz;
//       p7.x = _x_orig+_dx;
//       p7.y = _y_orig+_dy;
//       p7.z = _z_orig + _dz;
//       p8.x = _x_orig;
//       p8.y = _y_orig+_dy;
//       p8.z = _z_orig + _dz;
//       cloudOut->push_back(p1);
//       cloudOut->push_back(p2);
//       cloudOut->push_back(p3);
//       cloudOut->push_back(p4);
//       cloudOut->push_back(p5);
//       cloudOut->push_back(p6);
//       cloudOut->push_back(p7);
//       cloudOut->push_back(p8);

      return cloudOut;
    }

    inline int getSize(){return _my_pts.size();}

    inline double getDensity()
    {
      return ((double)_my_pts.size())/((_dx*_dy*_dz)/(_my_cube->getE3()));
    }

    inline CCluster splitMe()
    {
      CCluster newClust(_my_cube);
      CCluster backFillClust(_my_cube);

      std::vector<long>::iterator cIt;
      long ix,iy,iz;
      long z;
      long hitCount,lastHitCount = -1;
      double lastDens = 0;

      cout << "Trying to split" << endl;

      for (iz=0;iz<_my_cube->getZCount();iz++)
      {
	/*hitCount = 0;
	for (ix=0;ix<_my_cube->getXCount();ix++)
	{
	  for (iy=0;iy<_my_cube->getYCount();iy++)
	  {
	    if (_my_cube->isFilledIdx(_my_cube->toIdx(ix,iy,iz)))
	    {
	      hitCount++;
	    }
	  }
	}*/

	for (cIt = _my_pts.begin();cIt < _my_pts.end();
	      cIt++)
	{
	  z = (*cIt)/_my_cube->getXCount()/_my_cube->getYCount();
	  if (z == iz)
	  {
	    newClust.addIdx(*cIt);
	  }
	}
	if (iz > 1)
	{
	  newClust.calcDimensions();
	  if (lastDens > 0 && newClust.getDensity() < lastDens)
	  {
	    break;
	  }
	  lastDens = newClust.getDensity();
	}
      }

      newClust._my_pts.clear();
      if (iz < _my_cube->getZCount())
      {
	for (cIt = _my_pts.begin();cIt < _my_pts.end();
	      cIt++)
	{
	  z = (*cIt)/_my_cube->getXCount()/_my_cube->getYCount();
	  if (z < iz)
	  {
	    newClust.addIdx(*cIt);
	  }
	  else
	  {
	    backFillClust.addIdx(*cIt);
	  }
	}
	_my_pts.clear();
	for (cIt = backFillClust._my_pts.begin();cIt < backFillClust._my_pts.end();
	      cIt++)
	{
	  addIdx(*cIt);
	}
	backFillClust._my_pts.clear();
      }

      return newClust;
    }

    inline double getXMin(){return _x_orig;}
    inline double getYMin(){return _y_orig;}
    inline double getZMin(){return _z_orig;}
    inline double getXMax(){return _x_orig+_dx;}
    inline double getYMax(){return _y_orig+_dy;}
    inline double getZMax(){return _z_orig+_dz;}
    inline double getDX(){return _dx;}
    inline double getDY(){return _dy;}
    inline double getDZ(){return _dz;}

    inline std::vector<long> *getIdxs(){return &_my_pts;}

  private:
    std::vector<long> _my_pts;
    CubeAligner *_my_cube;
    double _x_orig;
    double _y_orig;
    double _z_orig;
    double _dx;
    double _dy;
    double _dz;
  };

  inline double getE3(){return _e*_e*_e;}

  inline void removeCluster(CCluster *cluster)
  {
    std::vector<long>::iterator clustIter;
    long idx,arrIdx;
    int bitIdx;

    for (clustIter = cluster->getIdxs()->begin();
	 clustIter < cluster->getIdxs()->end();
	 clustIter++)
    {
      idx = *clustIter;

      arrIdx = idx/8;
      bitIdx = idx%8;
      _vBase[arrIdx] ^= (0x80 >> bitIdx);
    }

    return;
  }

  inline void identifyClusters()
  {
    long ix,iy,iz,idx;
    std::vector<CCluster>::iterator clustIter;

    _clusters.clear();

    for (iz=0;iz<(-_min_z)/_e;iz++)
    {
      for (ix=0;ix<_xCount;ix++)
      {
	for (iy=0;iy<_yCount;iy++)
	{
	  idx = toIdx(ix,iy,iz);
	  if ( isFilledIdx(idx) )
	  {
	    bool found = false;
	    for (clustIter = _clusters.begin(); clustIter < _clusters.end();
		 clustIter++)
	    {
	      if ((*clustIter).includes(idx))
	      {
		found = true;
		break;
	      }
	    }
	    if (found)
	    {
	      continue;
	    }
	    CCluster newClust((CubeAligner*)this);
	    //growClusterRadial(idx,idx,&newClust);
	    growCluster(idx,&newClust);
	    if (newClust.getSize() > 25)
	    {
	      newClust.calcDimensions();
	      _clusters.push_back(newClust);
	    }
	  }
	}
      }
    }

    splitClusters();

    return;

    for (clustIter = _clusters.begin(); clustIter < _clusters.end();
	  clustIter++)
    {
      //if ((*clustIter).getZMin() < 0)
      //{
	removeCluster(&(*clustIter));
      //}
    }

    _clusters.clear();

    for (ix=0;ix<_xCount;ix++)
    {
      for (iy=0;iy<_yCount;iy++)
      {
	for (iz=0;iz<_zCount;iz++)
	{
	  idx = toIdx(ix,iy,iz);
	  if ( isFilledIdx(idx) )
	  {
	    bool found = false;
	    for (clustIter = _clusters.begin(); clustIter < _clusters.end();
		 clustIter++)
	    {
	      if ((*clustIter).includes(idx))
	      {
		found = true;
		break;
	      }
	    }
	    if (found)
	    {
	      continue;
	    }
	    CCluster newClust((CubeAligner*)this);
	    growCluster(idx,&newClust);
	    if (newClust.getSize() > 20)
	    {
	      newClust.calcDimensions();
	      //cout << "Added Cluster -> size = " << newClust.getSize() << endl;
	      found = false;
	      for (clustIter = _clusters.begin(); clustIter < _clusters.end();
		  clustIter++)
	      {
		if (
		  (*clustIter).getDensity() < newClust.getDensity() )
		{
		  _clusters.insert(clustIter,newClust);
		  found = true;
		  break;
		}
	      }
	      if (!found)
	      {
		_clusters.push_back(newClust);
	      }
	    }
	  }
	}
      }
    }

    cout << "Found " << _clusters.size() << " Clusters" << endl;

    return;
  }

  inline std::vector<CCluster> *getClusters()
  {
    return &_clusters;
  }

  inline void splitClusters()
  {
    std::vector<CCluster>::iterator clustIter;
    for (clustIter = _clusters.begin(); clustIter < _clusters.end();
	clustIter++)
    {
      if ((*clustIter).getDensity() < MIN_CLUSTER_DENS )
      {
	_clusters.push_back((*clustIter).splitMe());
      }
    }

    return;
  }

  inline void growClusterRadial(long startIdx, long thisIdx, CCluster *clust)
  {
    long ix,iy,iz,ix2,iy2,iz2;
    int dx,dy,dz;
    long newIdx;
    bool isThisFilled;

    ix = startIdx%_xCount;
    iy = (startIdx/_xCount)%_yCount;
    iz = startIdx/_xCount/_yCount;

    isThisFilled = isFilledIdx(thisIdx);
    if (isThisFilled)
    {
      if (!clust->includes(thisIdx))
      {
	clust->addIdx(thisIdx);
      }
      else
      {
	return;
      }
    }

    if (startIdx==thisIdx)
    {
      for (dx=-1;dx<=1;dx++)
      {
	for (dy=-1;dy<=1;dy++)
	{
	  if (!dx && !dy){continue;}
	  newIdx = toIdx(ix+dx,iy+dy,iz);
	  if (newIdx >=  0)
	    growClusterRadial(startIdx,newIdx,clust);
	}
      }
    }
    else
    {
      ix2 = thisIdx%_xCount;
      iy2 = (thisIdx/_xCount)%_yCount;
      iz2 = thisIdx/_xCount/_yCount;
      dx = ix2-ix;
      dy = iy2-iy;

      //cout << "checking " << ix2<<","<<iy2<<","<<iz2<<endl;
      //cout << "ctr = " << ix<<","<<iy<<","<<iz<<endl;

      if (dx)
      {
	newIdx = toIdx(ix2+(dx<0?-1:1),iy2,iz2);
	if ( newIdx >= 0 && (isFilledIdx(newIdx) || isThisFilled))
	  growClusterRadial(startIdx,newIdx,clust);
	/*if (!isFilledIdx(newIdx) && isThisFilled)
	{
	  newIdx = toIdx(ix2+(dx<0?-1:1),iy2,iz2+1);
	  if (isFilledIdx(newIdx) && !clust->includes(newIdx))
	    growClusterRadial(startIdx,newIdx,clust);
	  newIdx = toIdx(ix2+(dx<0?-1:1),iy2,iz2-1);
	  if (isFilledIdx(newIdx) && !clust->includes(newIdx))
	    growClusterRadial(startIdx,newIdx,clust);
	}*/
      }
      if (dy)
      {
	newIdx = toIdx(ix2,iy2+(dy<0?-1:1),iz2);
	if ( newIdx >= 0 && (isFilledIdx(newIdx) || isThisFilled))
	  growClusterRadial(startIdx,newIdx,clust);
	/*if (!isFilledIdx(newIdx) & isThisFilled)
	{
	  newIdx = toIdx(ix2,iy2+(dy<0?-1:1),iz2+1);
	  if (isFilledIdx(newIdx) && !clust->includes(newIdx))
	    growClusterRadial(startIdx,newIdx,clust);
	  newIdx = toIdx(ix2,iy2+(dy<0?-1:1),iz2-1);
	  if (isFilledIdx(newIdx) && !clust->includes(newIdx))
	    growClusterRadial(startIdx,newIdx,clust);
	}*/
      }
      if (dx && dy)
      {
	newIdx = toIdx(ix2+(dx<0?-1:1),iy2+(dy<0?-1:1),iz2);
	if ( newIdx >= 0 && (isFilledIdx(newIdx) || isThisFilled))
	  growClusterRadial(startIdx,newIdx,clust);
      }
    }

    return;
  }

  inline void growCluster(long startIdx, CCluster *clust)
  {
    long ix,iy,iz,idx;
    ix = startIdx%_xCount;
    iy = (startIdx/_xCount)%_yCount;
    iz = startIdx/_xCount/_yCount;
    int offset;

    clust->addIdx(startIdx);

    for (offset = -1; offset <= 1; offset += 2)
    {
      idx = toIdx(ix,iy+offset,iz);
      if (idx >= 0 && isFilledIdx(idx) && !clust->includes(idx))
      {
	growCluster(idx,clust);
      }
      else if ((idx = toIdx(ix,iy+offset*2,iz)) >= 0 && isFilledIdx(idx) &&
	!clust->includes(idx))
      {
	growCluster(idx,clust);
      }

      idx = toIdx(ix,iy,iz+offset);
      if (idx >= 0 && isFilledIdx(idx) && !clust->includes(idx))
      {
	growCluster(idx,clust);
      }
      else if ( (idx = toIdx(ix,iy,iz+offset*2)) >= 0 && isFilledIdx(idx) &&
      !clust->includes(idx))
      {
      growCluster(idx,clust);
      }

      //if (abs(ixStart - (ix+offset)) <= 3)
      //{
      idx = toIdx(ix+offset,iy,iz);
      if (idx >= 0 && isFilledIdx(idx) && !clust->includes(idx))
      {
	growCluster(idx,clust);
      }
      else if ((idx = toIdx(ix+offset*2,iy,iz)) >= 0 && isFilledIdx(idx) &&
	!clust->includes(idx))
      {
	growCluster(idx,clust);
      }
      //}
    }

    return;
  }

  inline void filterCloudCubic(CloudPtr cloud)
  {
    CloudItr npIt = cloud->begin();
    CloudPtr cloud2(new Cloud);
    ExtractIndices<PointXYZI> iextract(false);
    boost::shared_ptr<vector<int> > removeIndices(new vector<int>);
    long ix,iy,iz;
    int bitIdx;
    long long idx;
    size_t arrIdx;
    int i=0;
    int dropCount=0;

    removeIndices->reserve(cloud->size());

    while (npIt<cloud->end())
    {
      ix = ((*npIt).x-_min_x)/_e;
      iy = ((*npIt).y-_min_y)/_e;
      iz = ((*npIt).z-_min_z)/_e;

      if (isFilledIdx(toIdx(ix,iy,iz)))
      {
	removeIndices->push_back(i);
	dropCount++;
      }

      npIt++;
      i++;
    }
    //cout << "before " << cloud->size();
    iextract.setInputCloud(cloud);
    iextract.setNegative(true);
    iextract.setIndices(removeIndices);
    iextract.filter(*cloud2);
    cloud->swap(*cloud2);
    //cout << "after " << cloud->size() << endl;

    //cout << "Dropped " << dropCount << " non cubic points" << endl;
    return;
  }

  inline Eigen::Matrix4f alignToMatrix(Eigen::Matrix4f mat, CloudPtr cloud,
    double *eVal)
  {
    CloudPtr transCloud;
    CVertex x0;
    CVertex xr;
    CVertex xe;
    CVertex vertArr[V_CT];
    int i,j;
    int lCount=0;
    int cubeCount;
    int loopCount = 0;
    unsigned char *vArr;
    vArr = (unsigned char *)malloc(arraySizeNeeded());
    //try an initial offset in the direction of carts motion...
    CVertex v0(mat);
    double newScore;
    int didReduct;

    while(1)
    {
      loopCount++;
      for (i=0;i<V_CT;i++)
      {
	vertArr[i] = CVertex(i,*eVal,v0);
	transCloud = CloudPtr(new Cloud);
	transformPointCloud(*cloud,*transCloud,vertArr[i].toTranslation());
	memset(vArr,0,arraySizeNeeded());
	if (i==0){
	  cubeCount = cloudToArray(transCloud,vArr);
	}
	else
	{
	  cloudToArray(transCloud,vArr);
	}

	vertArr[i].setScore(getHitCount(vArr));
      }
      qsort(vertArr,V_CT,sizeof(CVertex),CVertex::sorter);
      didReduct = 0;

      while (1)
      {

	/*for (i=0;i<V_CT;i++)
	{
	  printf("%d,",vertArr[i].getScore());
	}
	printf("\n");*/
	for (i=0;i<V_CT;i++)
	{
	  //printf("%d,",vertArr[i].getScore());
	}
	//printf("\n");

	//calculate center of mass and reflection point
	x0 = CVertex(vertArr,V_CT-1);
	xr = x0 + ALPHA*(x0-vertArr[V_CT-1]);

	//calculate reflection points score
	transCloud = CloudPtr(new Cloud);
	transformPointCloud(*cloud,*transCloud,xr.toTranslation());
	memset(vArr,0,arraySizeNeeded());
	cloudToArray(transCloud,vArr);
	xr.setScore(getHitCount(vArr));

	for (i=0;i<V_CT;i++)
	{
	  if (xr.getScore() > vertArr[i].getScore())
	  {
	    break;
	  }
	}

	//replace the worst point with reflected and continue
	if ((i > 0 && i < V_CT-1) || xr.getScore() == vertArr[0].getScore())
	{
	  vertArr[V_CT-1] = xr;
	}
	else if (i==0)
	{
	  xe = x0 + BETA*(xr-x0);
	  //calculate expansion points score
	  transCloud = CloudPtr(new Cloud);
	  transformPointCloud(*cloud,*transCloud,xe.toTranslation());
	  memset(vArr,0,arraySizeNeeded());
	  cloudToArray(transCloud,vArr);
	  xe.setScore(getHitCount(vArr));
	  if (xe.getScore() > xr.getScore())
	  {
	    vertArr[V_CT-1] = xe;
	  }
	  else
	  {
	    vertArr[V_CT-1] = xr;
	  }
	}
	else if (i == (V_CT - 1))
	{
	  //outside contraction
	  xe = x0 + GAMMA*(vertArr[V_CT-1]-x0);
	  transCloud = CloudPtr(new Cloud);
	  transformPointCloud(*cloud,*transCloud,xe.toTranslation());
	  memset(vArr,0,arraySizeNeeded());
	  cloudToArray(transCloud,vArr);
	  xe.setScore(getHitCount(vArr));
	  if (xe.getScore()  > xr.getScore())
	  {
	    vertArr[V_CT-1] = xe;
	  }
	  else
	  {
	    i=-1;
	  }
	}
	else
	{
	  //inside contraction
	  xe = x0 - GAMMA*(vertArr[V_CT-1]-x0);
	  transCloud = CloudPtr(new Cloud);
	  transformPointCloud(*cloud,*transCloud,xe.toTranslation());
	  memset(vArr,0,arraySizeNeeded());
	  cloudToArray(transCloud,vArr);
	  xe.setScore(getHitCount(vArr));
	  if (xe.getScore()  > xr.getScore())
	  {
	    vertArr[V_CT-1] = xe;
	  }
	  else
	  {
	    i=-1;
	  }
	}

	if (i == -1)
	{
	  //reduction
	  if (didReduct == 5)
	  {
	    lCount = CUBE_MAX_ITER;
	  }
	  else
	  {
	    //reduce
	    for (i=1;i<V_CT;i++)
	    {
	      vertArr[i] = vertArr[0] + DELTA*(vertArr[i]-vertArr[0]);
	      transCloud = CloudPtr(new Cloud);
	      transformPointCloud(*cloud,*transCloud,vertArr[i].toTranslation());
	      memset(vArr,0,arraySizeNeeded());
	      cloudToArray(transCloud,vArr);
	      vertArr[i].setScore(getHitCount(vArr));
	    }

	    //cout << "REDUCED...";
	    didReduct++;
	  }
	}

	newScore = vertArr[V_CT-1].getScore();
	//printf("Next score = %d\n",vertArr[V_CT-1].getScore());

	qsort(vertArr,V_CT,sizeof(CVertex),CVertex::sorter);
	if (didReduct && i == V_CT)
	{
	  //dont quit early from std dev right after a reduction
	  newScore = vertArr[0].getScore();
	}

	//terminate the loop
	if (++lCount > CUBE_MAX_ITER)
	{
	  break;
	}
	if (newScore != vertArr[0].getScore() &&
	  getStdDev(vertArr,V_CT) < (vertArr[0].getScore()*.01))
	{
	  break;
	}
      }

      /*cout << "TESTING 123 " << loopCount << endl << endl << endl;

      //check the distance to the new and restart maybe??
      if (loopCount <= 3 && vertArr[0].getScore() < cubeCount*0.2)
      {
	lCount = 0;
	v0 = vertArr[0];
      }
      else
      {*/
	break;
      //}
    }

    x0 = vertArr[0];

    /*newScore = x0.getScore();
    for (j=0;j<10;j++)
    {
      lCount = 0;
      for (i=0;i<V_CT;i++)
      {
	xe = CVertex(i,E_VAL/100.0,x0);
	transCloud = CloudPtr(new Cloud);
	transformPointCloud(*cloud,*transCloud,xe.toTranslation());
	memset(vArr,0,arraySizeNeeded());
	cloudToArray(transCloud,vArr);
	xe.setScore(getHitCount(vArr));
	if (xe.getScore() > newScore)
	{
	  lCount = i;
	  newScore = xe.getScore();
	  xr = xe;
	}
	cout << "Offset " << i << " = " << xe.getScore() << endl;
      }
      if (lCount == 0)
      {
	break;
      }
      else
      {
	x0=xr;
      }
    }*/

    //E_VAL = ( E_VAL + x0.getMaxDiff(v0)*2 )/2.0;
    //if (E_VAL<0.05)E_VAL = 0.05;

    //*eVal = (*eVal + v0.getMaxDiff(x0))*0.5;
    //if (*eVal<0.01) *eVal = 0.01;

    //cout << "New E_VAL = " << *eVal << endl;
    //cout << "Starting point found:" << endl << v0 << endl;
    //cout << "Best trans = " << x0 << endl;

#ifdef ADAPT_SIMPLEX
    //cout << "ADAPTING!!>>!!!!!" << endl;
#endif

    free(vArr);

    return x0.toTranslation();
  }

  inline double getNextEval(Eigen::Matrix4f *guess, Eigen::Matrix4f *align)
  {
    CVertex c1(*guess);
    CVertex c2(*align);

    return c1.getMaxDiff(c2);
  }

  inline int getXCount(){return _xCount;}
  inline int getYCount(){return _yCount;}
  inline int getZCount(){return _zCount;}

private:

  inline CVertex findStartingPoint(CloudPtr cloud, double minX,
    double maxX, double minY, double maxY, double minZ, double maxZ,
    double stepSize)
  {
    CVertex best;
    CVertex test;
    CloudPtr transCloud;
    double x,y,z;
    unsigned char *vArr;
    vArr = (unsigned char *)malloc(arraySizeNeeded());

    best.setScore(0);

    for (x = minX;x<=maxX;x+=stepSize)
    {
      for (y = minY;y<=maxY;y+=stepSize)
      {
	for (z = minZ; z <= maxZ; z+=stepSize)
	{
	  test = CVertex(0,0,0,x,y,z);
	  cout << "testing " << test <<"..." << endl;
	  transCloud = CloudPtr(new Cloud);
	  transformPointCloud(*cloud,*transCloud,test.toTranslation());
	  memset(vArr,0,arraySizeNeeded());
	  cloudToArray(transCloud,vArr);
	  test.setScore(getHitCount(vArr));
	  if (test.getScore() > best.getScore())
	  {
	    best = test;
	  }
	}
      }
    }

    free(vArr);
    return best;
  }

  inline int getHitCount(unsigned char *v2)
  {
    int count = 0;
    int i,j;
    int byteCount = arraySizeNeeded();
    unsigned char cpByte;

    for (i=0;i<byteCount;i++)
    {
      cpByte = _vBase[i]&v2[i];
      for (j=0;j<8;j++)
      {
	count += ((cpByte >> j) & 0x01);
      }
    }

    return count;
  }

  inline bool isFilledIdx(long idx)
  {
    long arrIdx;
    int bitIdx;

    //catch out of range from toIdx()
    if (idx==-1)
    {
      return false;
    }

    arrIdx = idx/8;
    bitIdx = idx%8;
    return (_vBase[arrIdx] & (0x80 >> bitIdx) );
  }

  inline double getStdDev(CVertex *vertArr, int count)
  {
    double mean=0;
    double stdDev=0;
    int i;
    for (i=0;i<count;i++)
    {
      mean += vertArr[i].getScore();
    }
    mean /= count;

    for (i=0;i<count;i++)
    {
      stdDev += pow(vertArr[i].getScore()-mean,2.0)/(count-1);
    }

    stdDev = sqrt(stdDev);
    //printf("Std dev = %.2f\n",stdDev);
    return stdDev;
  }

  std::vector<CCluster> _clusters;
  double _min_x;
  double _max_x;
  double _min_y;
  double _max_y;
  double _min_z;
  double _max_z;
  double _e;
  long _xCount;
  long _yCount;
  long _zCount;
  size_t _arrSz;
  unsigned char* _vBase;
};

#endif


//
// Created by wei on 4/11/18.
//

#include "common/logging.h"
#include "core/warp_solver/solver_constants.h"
#include "core/warp_solver/geometry_icp_jacobian.cuh"
#include "core/warp_solver/ApplyJtJMatrixFreeHandler.h"

//Use for eigen
#include <Eigen/Eigen>
#include <Eigen/Sparse>

//This verion is deprecated, thus move here for sanity check purpose only
namespace surfelwarp { namespace device {

	__host__ __forceinline__ void computePointToPointICPTermJacobian(
		const float4& depth_vertex_confid,
		const float4& can_vertex4,
		const ushort4& knn, const float4& knn_weight,
		//The warp field
		const DualQuaternion* device_warp_field,
		const mat34& camera2world,
		//The output
		TwistGradientOfScalarCost* twist_gradient //[3]
	) {
		//Correct the size
		const float3 can_vertex = make_float3(can_vertex4.x, can_vertex4.y, can_vertex4.z);
		const float3 depth_vertex = make_float3(depth_vertex_confid.x, depth_vertex_confid.y, depth_vertex_confid.z);

		//Warp it
		DualQuaternion dq_average = averageDualQuaternion(device_warp_field, knn, knn_weight);
		const mat34 se3 = dq_average.se3_matrix();
		const float3 warped_vertex = se3.rot * can_vertex + se3.trans;

		//Compute the gradient
		twist_gradient[0].translation = make_float3(1.0f, 0.0f, 0.0f);
		twist_gradient[1].translation = make_float3(0.0f, 1.0f, 0.0f);
		twist_gradient[2].translation = make_float3(0.0f, 0.0f, 1.0f);
		twist_gradient[0].rotation = make_float3(0.0f, warped_vertex.z, -warped_vertex.y);
		twist_gradient[1].rotation = make_float3(-warped_vertex.z, 0.0f, warped_vertex.x);
		twist_gradient[2].rotation = make_float3(warped_vertex.y, -warped_vertex.x, 0.0f);
	}

} // device
} // surfelwarp

void surfelwarp::ApplyJtJHandlerMatrixFree::appendScalarCostJacobianTriplet(
	ScalarCostTerm2Jacobian& term2jacobian,
	unsigned row_offset,
	std::vector<Eigen::Triplet<float>>& jacobian_triplet,
	float term_weight
){
	//Download the required data
	std::vector<float4> knn_weight_array;
	std::vector<ushort4> knn_array;
	std::vector<TwistGradientOfScalarCost> twist_gradient_array;
	term2jacobian.knn_weight_array.Download(knn_weight_array);
	term2jacobian.knn_array.Download(knn_array);
	term2jacobian.twist_gradient_array.Download(twist_gradient_array);

	//Iterate over terms
	for(auto i = 0; i < knn_array.size(); i++) {
		const auto row_idx = i + row_offset;
		const ushort4 term_knn = knn_array[i];
		const float4 term_knn_weight = knn_weight_array[i];
		const TwistGradientOfScalarCost term_twist_gradient = twist_gradient_array[i];

		//Make them into flatten ptr
		const unsigned short* term_knn_arr = (const unsigned short*)(&term_knn);
		const float* term_knn_weight_arr = (const float*)(&term_knn_weight);
		const float* term_jacobian = (const float*)(&term_twist_gradient);

		//Iterates over knn
		for(auto j = 0; j < 4; j++) {
			auto node_idx = term_knn_arr[j];
			auto weight = term_knn_weight_arr[j];
			for(auto k = 0; k < 6; k++) {
				auto col_idx = k + 6 * node_idx;
				auto jacobian_value = term_weight * term_jacobian[k] * weight;
				jacobian_triplet.push_back(Eigen::Triplet<float>(row_idx, col_idx, jacobian_value));
			}
		}
	}
}

void surfelwarp::ApplyJtJHandlerMatrixFree::appendSmoothJacobianTriplet(std::vector<Eigen::Triplet<float>>& jacobian_triplet){
	//The row offset should be the number of depth terms
	auto num_depth_terms =  m_node2term_map.term_offset.DenseImageTermSize();
	auto row_offset = num_depth_terms;

	//Download the required data
	std::vector<ushort2> node_graph;
	std::vector<float4> node_coords;
	std::vector<DualQuaternion> node_se3;
	NodeGraphSmoothTerm2Jacobian smooth_term2jacobian = m_term2jacobian_map.smooth_term;
	smooth_term2jacobian.node_graph.Download(node_graph);
	smooth_term2jacobian.reference_node_coords.Download(node_coords);
	smooth_term2jacobian.node_se3.Download(node_se3);

	//Iterates through all node pairs
	for(auto k = 0; k < node_graph.size(); k++) {
		ushort2 node_ij = node_graph[k];
		float4 xi = node_coords[node_ij.x];
		float4 xj = node_coords[node_ij.y];
		mat34 Ti = node_se3[node_ij.x].se3_matrix();
		mat34 Tj = node_se3[node_ij.y].se3_matrix();
		TwistGradientOfScalarCost twist_gradient_i[3];
		TwistGradientOfScalarCost twist_gradient_j[3];
		device::computeSmoothTermJacobian(xj, Ti, Tj, twist_gradient_i, twist_gradient_j);

		//Iterate over channels
		for(auto channel = 0; channel < 3; channel++) {
			auto row_idx = row_offset + k * 3 + channel;

			//The first node in this channel
			float* jacobian = (float*)(&twist_gradient_i[channel]);
			for(auto j = 0; j < 6; j++) {
				auto node_idx = node_ij.x;
				auto col_idx = 6 * node_idx + j;
				auto value = lambda_smooth * jacobian[j];
				jacobian_triplet.push_back(Eigen::Triplet<float>(row_idx, col_idx, value));
			}

			//The second node in this channel
			jacobian = (float*)(&twist_gradient_j[channel]);
			for(auto j = 0; j < 6; j++) {
				auto node_idx = node_ij.y;
				auto col_idx = 6 * node_idx + j;
				auto value = lambda_smooth * jacobian[j];
				jacobian_triplet.push_back(Eigen::Triplet<float>(row_idx, col_idx, value));
			}
		} // iteration over channel
	} // iteration over terms
}

void surfelwarp::ApplyJtJHandlerMatrixFree::appendFeatureJacobianTriplet(
	unsigned row_offset,
	std::vector<Eigen::Triplet<float>>& jacobian_triplet
){
	//Download the required data
	auto term2jacobian_dev = m_term2jacobian_map.sparse_feature_term;
	std::vector<float4> depth_vertex_arr, reference_vertex_arr, knn_weight_arr;
	std::vector<ushort4> knn_arr;
	std::vector<DualQuaternion> node_se3_arr;
	term2jacobian_dev.target_vertex.Download(depth_vertex_arr);
	term2jacobian_dev.reference_vertex.Download(reference_vertex_arr);
	term2jacobian_dev.knn.Download(knn_arr);
	term2jacobian_dev.knn_weight.Download(knn_weight_arr);
	term2jacobian_dev.node_se3.Download(node_se3_arr);

	//Simple check
	SURFELWARP_CHECK_EQ(knn_arr.size(), depth_vertex_arr.size());
	SURFELWARP_CHECK_EQ(knn_arr.size(), reference_vertex_arr.size());
	SURFELWARP_CHECK_EQ(knn_arr.size(), knn_weight_arr.size());

	for(auto i = 0; i < knn_arr.size(); i++) {
		float4 depth_vertex = depth_vertex_arr[i];
		float4 ref_vertex = reference_vertex_arr[i];
		ushort4 knn = knn_arr[i];
		float4 knn_weight = knn_weight_arr[i];
		TwistGradientOfScalarCost twist_gradient[3];
		device::computePointToPointICPTermJacobian(
			depth_vertex,
			ref_vertex,
			knn, knn_weight,
			node_se3_arr.data(),
			mat34::identity(),
			twist_gradient
		);

		//Fill each knn
		unsigned short* knn_ushort = (unsigned short*)(&knn);
		float* weight_float = (float*)(&knn_weight);

		for(auto channel = 0; channel < 3; channel ++) {

			auto row_idx = row_offset + 3 * i + channel;
			const float* jacobian = (const float*)(&twist_gradient[channel]);

			for(auto j = 0; j < 4; j++) {
				auto node_idx = knn_ushort[j];
				auto node_weight = weight_float[j];
				for(auto k = 0; k < 6; k++) {
					auto col_idx = node_idx * 6 + k;
					auto value = lambda_feature * node_weight * jacobian[k];
					jacobian_triplet.push_back(Eigen::Triplet<float>(row_idx, col_idx, value));
				}
			}
		} // iterate over channels
	} // iterate over terms
}

void surfelwarp::ApplyJtJHandlerMatrixFree::applyJtJEigen(
	const std::vector<float> &x, unsigned num_scalar_terms,
	const std::vector<Eigen::Triplet<float>>& jacobian_triplet,
	std::vector<float> &jtj_x
){
	//Sanity check
	const auto num_nodes = m_node2term_map.offset.Size() - 1;
	SURFELWARP_CHECK_EQ(num_nodes * 6, x.size());
	SURFELWARP_CHECK_EQ(num_nodes * 6, jtj_x.size());

	//Construct the matrix
	Eigen::SparseMatrix<float> jacobian(num_scalar_terms, 6 * num_nodes);
	jacobian.setFromTriplets(jacobian_triplet.begin(), jacobian_triplet.end());

	//Construct the vector
	Eigen::VectorXf x_eigen; x_eigen.resize(x.size());
	for(auto i = 0; i < x.size(); i++) {
		x_eigen(i) = x[i];
	}

	//Perform jtj x
	Eigen::SparseMatrix<float> JtJ = Eigen::SparseMatrix<float>(jacobian.transpose()) * jacobian;
	Eigen::VectorXf jtj_x_eigen = JtJ * x_eigen;

	//Transfer back to std
	for(auto i = 0; i < jtj_x.size(); i++) {
		jtj_x[i] = jtj_x_eigen(i);
	}
}


#pragma once

#include "conditionals.hpp"

#include <vector>
#include <boost/math/distributions/gamma.hpp>

namespace lorad {

    class ASRV {

        public:
#if defined(POLGSS)
#if defined(HOLDER_ETAL_PRIOR)
            typedef std::vector<double>                 shape_refdist_t;
            typedef std::shared_ptr<shape_refdist_t>    shape_refdist_ptr_t;
#else
            typedef std::vector<double>                 ratevar_refdist_t;
            typedef std::shared_ptr<ratevar_refdist_t>  ratevar_refdist_ptr_t;
#endif
            typedef std::vector<double>                 pinvar_refdist_t;
            typedef std::shared_ptr<pinvar_refdist_t>   pinvar_refdist_ptr_t;
#endif

            typedef std::vector<double>                 rate_prob_t;
            typedef std::shared_ptr<double>             relrate_ptr_t;
#if defined(HOLDER_ETAL_PRIOR)
            typedef std::shared_ptr<double>             shape_ptr_t;
#else
            typedef std::shared_ptr<double>             ratevar_ptr_t;
#endif
            typedef std::shared_ptr<double>             pinvar_ptr_t;
            typedef std::shared_ptr<ASRV>               SharedPtr;

                                                ASRV();
            virtual                             ~ASRV();

            void                                clear();

            void                                setNumCateg(unsigned ncateg);
            unsigned                            getNumCateg() const;

#if defined(HOLDER_ETAL_PRIOR)
            void                                setShapeSharedPtr(shape_ptr_t shape);
            void                                setShape(double v);
            const shape_ptr_t                   getShapeSharedPtr() const;
            double                              getShape() const;
            void                                fixShape(bool is_fixed);
            bool                                isFixedShape() const;
#else
            void                                setRateVarSharedPtr(ratevar_ptr_t ratevar);
            void                                setRateVar(double v);
            const ratevar_ptr_t                 getRateVarSharedPtr() const;
            double                              getRateVar() const;
            void                                fixRateVar(bool is_fixed);
            bool                                isFixedRateVar() const;
#endif

            void                                setPinvarSharedPtr(pinvar_ptr_t pinvar);
            void                                setPinvar(double p);
            const pinvar_ptr_t                  getPinvarSharedPtr() const;
            double                              getPinvar() const;
            void                                fixPinvar(bool is_fixed);
            bool                                isFixedPinvar() const;

#if defined(POLGSS)
#if defined(HOLDER_ETAL_PRIOR)
            void                                setShapeRefDistParamsSharedPtr(shape_refdist_ptr_t shape_refdist_ptr);
            std::vector<double>                 getShapeRefDistParamsVect() const;
#else
            void                                setRateVarRefDistParamsSharedPtr(ratevar_refdist_ptr_t ratevar_refdist_ptr);
            std::vector<double>                 getRateVarRefDistParamsVect() const;
#endif
            void                                setPinvarRefDistParamsSharedPtr(pinvar_refdist_ptr_t pinvar_refdist_ptr);
            std::vector<double>                 getPinvarRefDistParamsVect() const;
#endif

            void                                setIsInvarModel(bool is_invar_model);
            bool                                getIsInvarModel() const;

            const double *                      getRates() const;
            const double *                      getProbs() const;

        private:

            virtual void                        recalcASRV();

            unsigned                            _num_categ;
            bool                                _invar_model;

#if defined(HOLDER_ETAL_PRIOR)
            bool                                _shape_fixed;
            shape_ptr_t                         _shape;
#else
            bool                                _ratevar_fixed;
            ratevar_ptr_t                       _ratevar;
#endif
            pinvar_ptr_t                        _pinvar;

            bool                                _pinvar_fixed;

            rate_prob_t                         _rates;
            rate_prob_t                         _probs;
#if defined(POLGSS)
#if defined(HOLDER_ETAL_PRIOR)
            shape_refdist_ptr_t                 _shape_refdist;
#else
            ratevar_refdist_ptr_t               _ratevar_refdist;
#endif
            pinvar_refdist_ptr_t                _pinvar_refdist;
#endif
    };

    inline ASRV::ASRV() {
        clear();
    }

    inline ASRV::~ASRV() {
    }

    inline void ASRV::clear() {
        // Rate homogeneity is the default
        _invar_model = false;
#if defined(HOLDER_ETAL_PRIOR)
        _shape_fixed = false;
        _shape = std::make_shared<double>(1.0);
#else
        _ratevar_fixed = false;
        _ratevar = std::make_shared<double>(1.0);
#endif
        _pinvar_fixed = false;
        _pinvar = std::make_shared<double>(0.0);
        _num_categ = 1;
#if defined(POLGSS)
#if defined(HOLDER_ETAL_PRIOR)
        ASRV::shape_refdist_t tmp = {1.0, 1.0};
        _shape_refdist = std::make_shared<ASRV::shape_refdist_t>(tmp);
#else
        ASRV::ratevar_refdist_t tmp = {1.0, 1.0};
        _ratevar_refdist = std::make_shared<ASRV::ratevar_refdist_t>(tmp);
#endif
        ASRV::pinvar_refdist_t tmp2 = {1.0, 1.0};
        _pinvar_refdist = std::make_shared<ASRV::pinvar_refdist_t>(tmp2);
#endif
        recalcASRV();
    }

#if defined(HOLDER_ETAL_PRIOR)
    inline const ASRV::shape_ptr_t ASRV::getShapeSharedPtr() const {
        return _shape;
    }

    inline double ASRV::getShape() const {
        assert(_shape);
        return *_shape;
    }

    inline void ASRV::setShapeSharedPtr(shape_ptr_t shape) {
        _shape = shape;
        recalcASRV();
    }

    inline void ASRV::setShape(double v) {
        *_shape = v;
        recalcASRV();
    }

    inline void ASRV::fixShape(bool is_fixed) {
        _shape_fixed = is_fixed;
    }

    inline bool ASRV::isFixedShape() const {
        return _shape_fixed;
    }
#else
    inline const ASRV::ratevar_ptr_t ASRV::getRateVarSharedPtr() const {
        return _ratevar;
    }

    inline double ASRV::getRateVar() const {
        assert(_ratevar);
        return *_ratevar;
    }

    inline void ASRV::setRateVarSharedPtr(ratevar_ptr_t ratevar) {
        _ratevar = ratevar;
        recalcASRV();
    }

    inline void ASRV::setRateVar(double v) {
        *_ratevar = v;
        recalcASRV();
    }

    inline void ASRV::fixRateVar(bool is_fixed) {
        _ratevar_fixed = is_fixed;
    }

    inline bool ASRV::isFixedRateVar() const {
        return _ratevar_fixed;
    }
#endif

    inline const ASRV::pinvar_ptr_t ASRV::getPinvarSharedPtr() const {
        return _pinvar;
    }

    inline double ASRV::getPinvar() const {
        assert(_pinvar);
        return *_pinvar;
    }

    inline const double * ASRV::getRates() const {
        return &_rates[0];
    }

    inline const double * ASRV::getProbs() const {
        return &_probs[0];
    }

    inline bool ASRV::getIsInvarModel() const {
        return _invar_model;
    }

    inline unsigned ASRV::getNumCateg() const {
        return _num_categ;
    }

    inline void ASRV::setNumCateg(unsigned ncateg) {
        _num_categ = ncateg;
        recalcASRV();
    }

    inline void ASRV::setPinvarSharedPtr(pinvar_ptr_t pinvar) {
        _pinvar = pinvar;
        recalcASRV();
    }

    inline void ASRV::setPinvar(double p) {
        *_pinvar = p;
        recalcASRV();
    }

    inline void ASRV::setIsInvarModel(bool is_invar_model) {
        _invar_model = is_invar_model;
        recalcASRV();
    }

    inline void ASRV::fixPinvar(bool is_fixed) {
        _pinvar_fixed = is_fixed;
    }

    inline bool ASRV::isFixedPinvar() const {
        return _pinvar_fixed;
    }

    inline void ASRV::recalcASRV() {
        // This implementation assumes discrete gamma among-site rate heterogeneity
        // using a _num_categ category discrete gamma distribution with equal category
        // probabilities and Gamma density with mean 1.0 and variance _rate_var.
        // If _invar_model is true, then rate probs will sum to 1 - _pinvar rather than 1
        // and the mean rate will be 1/(1 - _pinvar) rather than 1; the rest of the invariable
        // sites component of the model is handled outside the ASRV class.

        // _num_categ, _rate_var, and _pinvar must all have been assigned in order to compute rates and probs
#if defined(HOLDER_ETAL_PRIOR)
        if ( (!_shape) || (!_num_categ) || (!_pinvar) )
            return;
#else
        if ( (!_ratevar) || (!_num_categ) || (!_pinvar) )
            return;
#endif

        double pinvar = *_pinvar;
        assert(pinvar >= 0.0);
        assert(pinvar <  1.0);

        assert(_num_categ > 0);

        double equal_prob = 1.0/_num_categ;
        double mean_rate_variable_sites = 1.0;
        if (_invar_model)
            mean_rate_variable_sites /= (1.0 - pinvar);

        _rates.assign(_num_categ, mean_rate_variable_sites);
        _probs.assign(_num_categ, equal_prob);

#if defined(HOLDER_ETAL_PRIOR)
        double gamma_shape = *_shape;
        assert(gamma_shape >= 0.0);

        if (_num_categ == 1 || gamma_shape > 1000.0)
            return;

        double alpha = gamma_shape;
        double beta = 1.0/gamma_shape;
#else
        double rate_variance = *_ratevar;
        assert(rate_variance >= 0.0);

        if (_num_categ == 1 || rate_variance == 0.0)
            return;

        double alpha = 1.0/rate_variance;
        double beta = rate_variance;
#endif

        boost::math::gamma_distribution<> my_gamma(alpha, beta);
        boost::math::gamma_distribution<> my_gamma_plus(alpha + 1.0, beta);

        double cum_upper        = 0.0;
        double cum_upper_plus   = 0.0;
        double upper            = 0.0;
        double cum_prob         = 0.0;
        for (unsigned i = 1; i <= _num_categ; ++i) {
            double cum_lower_plus       = cum_upper_plus;
            double cum_lower            = cum_upper;
            cum_prob                    += equal_prob;

            if (i < _num_categ) {
                upper                   = boost::math::quantile(my_gamma, cum_prob);
                cum_upper_plus          = boost::math::cdf(my_gamma_plus, upper);
                cum_upper               = boost::math::cdf(my_gamma, upper);
            }
            else {
                cum_upper_plus          = 1.0;
                cum_upper               = 1.0;
            }

            double numer                = cum_upper_plus - cum_lower_plus;
            double denom                = cum_upper - cum_lower;
            double r_mean               = (denom > 0.0 ? (alpha*beta*numer/denom) : 0.0);
            _rates[i-1]        = r_mean*mean_rate_variable_sites;
        }
    }

#if defined(POLGSS)
#if defined(HOLDER_ETAL_PRIOR)
    inline void ASRV::setShapeRefDistParamsSharedPtr(ASRV::shape_refdist_ptr_t shape_refdist_params_ptr) {
        if (shape_refdist_params_ptr->size() != 2)
            throw XLorad(boost::format("Expecting 2 shape reference distribution parameters and got %d") % shape_refdist_params_ptr->size());
        _shape_refdist = shape_refdist_params_ptr;
    }

    inline std::vector<double> ASRV::getShapeRefDistParamsVect() const {
        return std::vector<double>(_shape_refdist->begin(), _shape_refdist->end());
    }
#else
    inline void ASRV::setRateVarRefDistParamsSharedPtr(ASRV::ratevar_refdist_ptr_t ratevar_refdist_params_ptr) {
        if (ratevar_refdist_params_ptr->size() != 2)
            throw XLorad(boost::format("Expecting 2 rate variance reference distribution parameters and got %d") % ratevar_refdist_params_ptr->size());
        _ratevar_refdist = ratevar_refdist_params_ptr;
    }

    inline std::vector<double> ASRV::getRateVarRefDistParamsVect() const {
        return std::vector<double>(_ratevar_refdist->begin(), _ratevar_refdist->end());
    }
#endif

    inline void ASRV::setPinvarRefDistParamsSharedPtr(ASRV::pinvar_refdist_ptr_t pinvar_refdist_params_ptr) {
        if (pinvar_refdist_params_ptr->size() != 2)
            throw XLorad(boost::format("Expecting 2 pinvar reference distribution parameters and got %d") % pinvar_refdist_params_ptr->size());
        _pinvar_refdist = pinvar_refdist_params_ptr;
    }

    inline std::vector<double> ASRV::getPinvarRefDistParamsVect() const {
        return std::vector<double>(_pinvar_refdist->begin(), _pinvar_refdist->end());
    }
#endif
}