Index: /applications/robust/robustlib.h
===================================================================
--- /applications/robust/robustlib.h (revision 1331)
+++ /applications/robust/robustlib.h (revision 1334)
@@ -1791,4 +1791,6 @@
 		if(conditions.size()-2-number_of_parameters>=0)
 		{
+			
+			
 			mat sample_mat;
 			map<double,toprow*> ordered_toprows;			
@@ -1805,4 +1807,6 @@
 			while(sample_mat.cols()<n)
 			{
+				cout << "*************************************" << endl;
+				
 				double rnumber = randu()*sum_a;
 
@@ -1810,8 +1814,10 @@
 
 				// This could be more efficient (log n), but map::upper_bound() doesn't let me dereference returned iterator
+				int toprow_count = 0;
 				toprow* sampled_toprow;				
 				for(map<double,toprow*>::iterator top_ref = ordered_toprows.begin();top_ref!=ordered_toprows.end();top_ref++)
 				{
 					// cout << "CDF:"<< (*top_ref).first << endl;
+					toprow_count++;
 
 					if((*top_ref).first >= rnumber)
@@ -1822,4 +1828,5 @@
 				}				
 
+				cout << "Toprow/Count: " << toprow_count << "/" << ordered_toprows.size() << endl;
 				// cout << &sampled_toprow << ";";
 
@@ -1828,6 +1835,9 @@
 				set<simplex*>::iterator s_ref;
 				double sum_b = 0;
+				int simplex_count = 0;
 				for(s_ref = sampled_toprow->triangulation.begin();s_ref!=sampled_toprow->triangulation.end();s_ref++)
 				{
+					simplex_count++;
+					
 					sum_b += (*s_ref)->probability;
 
@@ -1836,4 +1846,5 @@
 				}
 
+				cout << "Simplex/Count: " << simplex_count << "/" << sampled_toprow->triangulation.size() << endl;
 				// cout << &(*tri_ref) << endl;
 
@@ -1903,26 +1914,37 @@
 				for(set<vertex*>::iterator vert_ref = ++(*s_ref)->vertices.begin();vert_ref!=(*s_ref)->vertices.end();vert_ref++)
 				{
-					jacobian.set_row(row_count,(*vert_ref)->get_coordinates()-base_vert->get_coordinates());
+					vec relative_coords = (*vert_ref)->get_coordinates()-base_vert->get_coordinates();				
+
+					jacobian.set_row(row_count,relative_coords);
 
 					row_count++;
 				}
-
+				
+				cout << "Jacobian: " << jacobian << endl;
+
+				cout << "Gradient before trafo:" << gradient << endl;
+								
+				gradient = jacobian*gradient;	
+
+				// vec normal_gradient = gradient/sqrt(gradient*gradient);
 				// cout << gradient << endl;
-				// cout << jacobian << endl;
-
-				gradient = jacobian*gradient;	
-
-				//cout << gradient << endl;
-
-				mat rotation_matrix = eye(dimension);
-
-				double squared_length = 1;
+				// cout << normal_gradient << endl;
+				// cout << sqrt(gradient*gradient) << endl;
+
+				mat rotation_matrix = eye(dimension);				
+
+								
 
 				for(int i = 1;i<dimension;i++)
 				{
-					double t = gradient[i]/sqrt(squared_length);
-
-					double sin_theta = t/sqrt(1+pow(t,2));
-					double cos_theta = 1/sqrt(1+pow(t,2));
+					vec x_axis = zeros(dimension);
+					x_axis.set(0,1);
+
+					x_axis = rotation_matrix*x_axis;
+
+					double t = gradient[i]/gradient*x_axis;
+
+					double sin_theta = sign(gradient[i])*t/sqrt(1+pow(t,2));
+					double cos_theta = sign(gradient*x_axis)/sqrt(1+pow(t,2));
 
 					mat partial_rotation = eye(dimension);
@@ -1934,11 +1956,10 @@
 					partial_rotation.set(i,0,-sin_theta);
 					
-					rotation_matrix = rotation_matrix*partial_rotation;
-					
-					squared_length += pow(gradient[i],2);
+					rotation_matrix = rotation_matrix*partial_rotation;				
+					
 				}
 
 				// cout << rotation_matrix << endl;
-				// cout << gradient << endl;
+				cout << "Gradient after trafo:" << gradient << endl;
 
 				vec minima = numeric_limits<double>::max()*ones(number_of_parameters);
@@ -1947,5 +1968,5 @@
 				vertex* min_grad;
 				vertex* max_grad;
-				
+								
 				for(set<vertex*>::iterator vert_ref = (*s_ref)->vertices.begin();vert_ref!=(*s_ref)->vertices.end();vert_ref++)
 				{
@@ -1978,5 +1999,5 @@
 				}
 
-				vec sample_coordinates;
+				vec sample_coordinates;				
 
 				for(int j = 0;j<number_of_parameters;j++)
@@ -1990,6 +2011,13 @@
 						double pos_value = sampled_toprow->condition_sum.right(sampled_toprow->condition_sum.size()-1)*max_grad->get_coordinates()-sampled_toprow->condition_sum[0];
 						
+						vec new_gradient = rotation_matrix*gradient;
+
+						cout << "New gradient(should have only first component nonzero):" << new_gradient << endl;
+
+						cout << "Max: " << maxima[0] << "  Min: " << minima[0] << "  Grad:" << new_gradient[0] << endl;
 						
-						//coordinate = log(rnumber*(exp(sampled_toprow->condition_sum.size()-1)*max_grad->get_coordinates()-sampled_toprow->condition_sum[0])-exp(sampled_toprow->condition_sum.right(sampled_toprow->condition_sum.size()-1)*min_grad->get_coordinates()-sampled_toprow->condition_sum[0]))+
+						double log_bracket = rnumber*exp(new_gradient[0]*maxima[0]/sigma)+(1-rnumber)*exp(new_gradient[0]*minima[0]/sigma);
+						
+						coordinate = log(log_bracket);
 					}
 				}
