#include "robustlib.h"


double polyhedron::triangulate(bool should_integrate)
	{
		for(set<simplex*>::iterator t_ref = triangulation.begin();t_ref!=triangulation.end();t_ref++)
		{
			delete (*t_ref);
		}		
		triangulation.clear();

		if(should_integrate)
		{
			((toprow *)this)->probability = 0.0;
		}		
		
		if(vertices.size()==1)
		{
			simplex* vert_simplex = new simplex((vertex*)this);			

			triangulation.insert(vert_simplex);
		}

		for(list<polyhedron*>::iterator child_ref = children.begin();child_ref!=children.end();child_ref++)
		{			
			for(set<simplex*>::iterator s_ref = (*child_ref)->triangulation.begin();s_ref!=(*child_ref)->triangulation.end();s_ref++)
			{
				simplex* new_simplex = new simplex((*s_ref)->vertices);								

				pair<set<vertex*>::iterator,bool> ret_val = new_simplex->vertices.insert(*vertices.begin());

				if(ret_val.second == true)
				{
					double cur_prob = 0;

					/*
					if(should_integrate&&new_simplex->vertices.size()!=3)
					{
						cout << "Error: Wrong vertex count for integration!";
					}
					*/

					if(should_integrate)
					{
						cur_prob = ((toprow *)this)->integrate_simplex(new_simplex, 'S');

						((toprow *)this)->probability += cur_prob;
					}

					triangulation.insert(new_simplex);
				}
				else
				{
					delete new_simplex;
				}
			}	
		}	

		if(should_integrate)
		{
			return ((toprow *)this)->probability;
		}
		else
		{
			return 0.0;
		}
		
		/*
		for(list<polyhedron*>::iterator child_ref = children.begin();child_ref!=children.end();child_ref++)
		{			
			for(list<set<vertex*>>::iterator t_ref = (*child_ref)->triangulation.begin();t_ref!=(*child_ref)->triangulation.end();t_ref++)
			{
				set<vertex*> new_simplex;
				new_simplex.insert((*t_ref).begin(),(*t_ref).end());			

				for(set<vertex*>::iterator suitable_vert_ref = vertices.begin();*suitable_vert_ref<*(*t_ref).begin();suitable_vert_ref++)
				{
					set<vertex*> suitable_simplex;
					suitable_simplex.insert(new_simplex.begin(),new_simplex.end());
					
					suitable_simplex.insert(*suitable_vert_ref);

					triangulation.push_back(suitable_simplex);

					if(should_integrate)
					{
						((toprow *)this)->probability += ((toprow *)this)->integrate_simplex(suitable_simplex, 'S');
					}
				}				
			}	
		}*/		
	}


	double toprow::integrate_simplex(simplex* simplex, char c)
	{
		// cout << ((toprow*)this)->condition << endl;		
		int sigma_order = ((toprow*)this)->condition_order-simplex->vertices.size()-1;

		// cout << "C:" << condition_order << "  N:" << my_emlig->number_of_parameters << "  C+N:" << condition_order-my_emlig->number_of_parameters << endl;
		// pause(0.1);

		if(sigma_order >= 0)
		{

			//cout << endl;
			//cout << ((toprow*)this)->condition << endl;
			//cout << "C:" << condition_order+2 << "  N:" << my_emlig->number_of_parameters << "  C+N:" << condition_order-my_emlig->number_of_parameters+2 << endl;
			

			emlig* current_emlig;
			simplex->clear_gammas();
			simplex->probability = 0;

			if(this->my_emlig!=NULL)
			{
				current_emlig = this->my_emlig;
			}
			else
			{
				throw exception("The statistic of the polyhedron you are trying to integrate over doesn't belong to any emlig!");
			}						

			toprow* as_toprow = (toprow*)this;	

			//cout << "Current condition:" << as_toprow->condition_sum << endl;

			int dimension = simplex->vertices.size()-1;

			mat jacobian(dimension,dimension);			

			map<double,int> as;
			vertex* base_vertex = *simplex->vertices.begin();							
				
			int row_count = 0;
			for(set<vertex*>::iterator vert_ref = simplex->vertices.begin(); vert_ref!=simplex->vertices.end();vert_ref++)
			{
				if(vert_ref!=simplex->vertices.begin())
				{
					vec relative_coords = (*vert_ref)->get_coordinates()-base_vertex->get_coordinates();
					jacobian.set_row(row_count-1,relative_coords);
				}

				vec extended_coords = (*vert_ref)->get_coordinates();
				extended_coords.ins(0,-1);

				//cout << "Ext. coords:" << extended_coords << "  Condition sum:" << as_toprow->condition_sum << endl;

				double a = extended_coords*as_toprow->condition_sum;
				if(a<current_emlig->min_ll)
				{
					current_emlig->minimal_vertex = (*vert_ref);
					current_emlig->min_ll = a;						
				}

				//cout << "a0:" << a_0 << " a0 coords:" << base_vertex->get_coordinates() << " am:" << a_m << " am coords:" << (*vert_ref)->get_coordinates() << endl;

				//cout << "Absolute coords:(V"  << row_count << ")" << (*vert_ref)->get_coordinates() << endl;
				//cout << "Relative coords:(V"  << row_count << ")" << relative_coords << endl;

				pair<map<double,int>::iterator,bool> returned = as.insert(pair<double,int>(a,1));
				if(returned.second == false)
				{
					(*returned.first).second++;
				}
				
				row_count++;
			}		
			
			/*
			int as_count = 1;
			for(map<double,int>::iterator as_ref = as.begin();as_ref!=as.end();as_ref++)
			{
				cout << "a_" << as_count << ": " << (*as_ref).first << "    ";
				as_count++;
			}
			*/		

			vector<double> gamma_facs;
			// cout << jacobian << endl;
			double det_jacobian    = abs(det(jacobian));			
			for(map<double,int>::iterator a_ref = as.begin();a_ref!=as.end();a_ref++)
			{
				double k_multiplier = 1;			
				
				int a_order = (*a_ref).second;			
				current_emlig->set_correction_factors(a_order-1);

				vector<double> factors;												
				for(map<double,int>::iterator a2_ref = as.begin();a2_ref!=as.end();a2_ref++)
				{					
					if(a2_ref!=a_ref)
					{										
						for(int k = 0;k<(*a2_ref).second;k++)
						{
							factors.push_back((*a2_ref).first-(*a_ref).first);
						}

						k_multiplier /= pow((*a2_ref).first-(*a_ref).first,(*a2_ref).second);						
					}										
				}		

				double bracket_combination = 0;

				for(int k = 0;k < a_order;k++)
				{
					if(k==gamma_facs.size())
					{
						gamma_facs.push_back(0.0);
					}
					
					double factor_multiplier = pow((double)-1,k)/pow((*a_ref).first,sigma_order-a_order+k+1);//pow((double)-1,k+1)
										
					double value = 0;
					if(k!=0)
					{							
						ivec control_vec = ivec();
						control_vec.ins(0,my_emlig->number_of_parameters-a_order+1);						
						for(multiset<my_ivec>::iterator combi_ref = this->my_emlig->correction_factors[a_order-k-1].begin();combi_ref!=this->my_emlig->correction_factors[a_order-k-1].upper_bound(my_ivec(control_vec));combi_ref++)
						{
							double bracket_factor = 1/fact(k);
							 
							for(int j = 0;j<(*combi_ref).size();j++)
							{
								//cout << "Factor vector:" << (*combi_ref) << endl;
								
								bracket_factor /= factors[(*combi_ref)[j]-1];
							}
													
							value += bracket_factor*factor_multiplier*k_multiplier;							
						}						
					}
					else
					{
						value = k_multiplier*factor_multiplier;							
					}

					simplex->insert_gamma(k,value,(*a_ref).first);
					gamma_facs[k] += value;
				}				
			}

			double int_value = 0;
			double volume = det_jacobian/fact(jacobian.rows());
			int gamma_size = (int)gamma_facs.size();
			if(sigma_order-gamma_size>=0)
			{								
				for(int k = 0;k<gamma_size;k++)
				{	
					int_value += volume*fact(sigma_order+k)*gamma_facs[k]/pow(2.0,((toprow*)this)->condition_order);				
				}	
			}
			else
			{
				int_value = -1;
			}
			
			simplex->probability = int_value;
			simplex->volume = volume;
			//cout << "Probability:" << int_value << endl;
			//pause(0.1);
			return int_value;		
		}
		else
		{
			cout << "Improper probability density." << endl;
			return 0.0;
		}
	}