#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <assert.h>

#include "asdf/asdf.h"
#include "asdf/cache.h"
#include "util/interval.h"
#include "util/plane.h"
#include "quadrature.h"
#include "bspline.h"
#include "util/constants.h"
#include "integration.h"

/* FACES[i] gives the four vertices (as 3-bit corner indices)
that define face i on an ASDF cell. 
face order is -x,+x,-y,+y,-z,+z
vertex order is by RHR with positive normal axis, starting lowest
*/
static const uint8_t FACES[6][4] = {
		{0,2,3,1},{4,6,7,5}, //-+x
		{0,1,5,4},{2,3,7,6}, //-+y
		{0,4,6,2},{1,5,7,3}	 //-+z
};
/*
FACE_NORMAL[i] gives an unsigned normal vector n
n^v gives the vertex connected in the normal direction
*/
static const uint8_t FACE_NORMAL[6] = {4,4,2,2,1,1};
static const Vec3f FACE_NORMAL_VEC[6] = {
	(Vec3f){1,0,0},	(Vec3f){-1,0,0},
	(Vec3f){0,1,0},	(Vec3f){0,-1,0},
	(Vec3f){0,0,1},	(Vec3f){0,0,-1}
	};

static const uint8_t EDGES[12][2] = {
	{0,1},{0,2},{0,4},
	{1,3},{1,5},
	{2,3},{2,6},
	{3,7},
	{4,5},{4,6},
	{5,7},
	{6,7}
	};

/*
Vec3f edge_vec(const int e)
{
	const uint8_t d = EDGES[e][1]-EDGES[e][0]
	switch d{
		case 1:
			return (Vec3f){0,0,1};
		case 2:
			return (Vec3f){0,1,0};
		case 4:
			return (Vec3f){1,0,0};
		default:
			assert(0); //didn't find edge
			return (Vec3f){0,0,0};
	}
}
*/

/**
#brief Return Vec3f of a vertex by 3 bit index
*/
Vec3f asdf_vertex_point(const ASDF* const asdf,const int v)
{
	return (Vec3f){
		(v & 4) ? asdf->X.upper : asdf->X.lower,
		(v & 2) ? asdf->Y.upper : asdf->Y.lower,
		(v & 1) ? asdf->Z.upper : asdf->Z.lower
		};
}
float asdf_cell_dimension(const ASDF* const asdf, const uint8_t n)
{
	if      (n & 4){return asdf->X.upper - asdf->X.lower;}
	else if (n & 2){return asdf->Y.upper - asdf->Y.lower;}
	else if (n & 1){return asdf->Z.upper - asdf->Z.lower;}
	else { assert(0); return 0;} //not a valid normal direction
}
float asdf_vertex_distance(const ASDF* const asdf, const uint8_t v0, const uint8_t v1)
{
	return norm(vdiff(asdf_vertex_point(asdf,v0),asdf_vertex_point(asdf,v1)));
}



/**
@brief Return volume of ASDF cell
*/
float asdf_volume(const ASDF* const asdf)
{
    if (asdf == NULL)   return 0.;
    return (asdf->X.upper - asdf->X.lower)*(asdf->Y.upper - asdf->Y.lower)*(asdf->Z.upper - asdf->Z.lower);
}
/**
@brief Return area of face of ASDF cell
*/
float asdf_face_area(const ASDF* const asdf, const int f)
{
    if (asdf == NULL)   return 0.;
    if      (FACE_NORMAL[f] & 4){ return (asdf->Y.upper - asdf->Y.lower)*(asdf->Z.upper - asdf->Z.lower);}
    else if (FACE_NORMAL[f] & 2){ return (asdf->X.upper - asdf->X.lower)*(asdf->Z.upper - asdf->Z.lower);}
    else if (FACE_NORMAL[f] & 1){ return (asdf->X.upper - asdf->X.lower)*(asdf->Y.upper - asdf->Y.lower);}
    else { return 0;}
}
/**
@brief Return length of ASDF edge
*/
float asdf_edge_length(const ASDF* const asdf,const int e)
{
	return asdf_vertex_distance(asdf,EDGES[e][0],EDGES[e][1]);
}


/**scalar linear interpolation*/
float linear_interp(float*v, float sx)
{
	return .5*(v[0]*(1-sx)+
						 v[1]*(1+sx));
}

/**scalar bilinear interpolation*/
float bilinear_interp(float* v, float sx, float sy)
{
	return .25*(v[0]*(1-sy)*(1-sx)+
							v[1]*(1+sy)*(1-sx)+
							v[2]*(1-sy)*(1+sx)+
							v[3]*(1+sy)*(1+sx));
}
/**scalar trilinear interpolation*/
float trilinear_interp(float* v, float sx, float sy, float sz)
{
	return .125*(v[0]*(1-sz)*(1-sy)*(1-sx)+
							 v[1]*(1+sz)*(1-sy)*(1-sx)+
							 v[2]*(1-sz)*(1+sy)*(1-sx)+
							 v[3]*(1+sz)*(1+sy)*(1-sx)+
							 v[4]*(1-sz)*(1-sy)*(1+sx)+
							 v[5]*(1+sz)*(1-sy)*(1+sx)+
							 v[6]*(1-sz)*(1+sy)*(1+sx)+
							 v[7]*(1+sz)*(1+sy)*(1+sx));
}
Vec3f linear_point(Vec3f p0, Vec3f p1, float sx)
{
	return vscale(.5, vsum( vscale(1-sx,p0), vscale(1+sx,p1) ));
}

Vec3f trilinear_point(Vec3f* p, float sx, float sy, float sz)
{
	float px[8], py[8], pz[8]; //gross
	for(int i=0; i<8; ++i){px[i] = p[i].x; py[i] = p[i].y; pz[i] = p[i].z;}
	return (Vec3f){
		trilinear_interp(px,sx,sy,sz),
		trilinear_interp(py,sx,sy,sz),
		trilinear_interp(pz,sx,sy,sz)
	};
}


/**
@brief return an interior point of the ASDF based on coordinate values in [-1,1]
@details wx, wy, wx take values in [-1,1] from min to max
*/
Vec3f asdf_interior_point(const ASDF* const asdf, 
	const float wx, const float wy, const float wz)
{
	return (Vec3f){
		.5*(asdf->X.upper+asdf->X.lower + wx*(asdf->X.upper-asdf->X.lower)),
		.5*(asdf->Y.upper+asdf->Y.lower + wy*(asdf->Y.upper-asdf->Y.lower)),
		.5*(asdf->Z.upper+asdf->Z.lower + wz*(asdf->Z.upper-asdf->Z.lower))
	};
}

/**
@brief return an interior point of the ASDF based on coordinate values in [-1,1 ]with reference to a face
@details w1, w2, w3 take values in [-1,1] from min to max
w1 and w2 are associated with first and second axes of the face f
w3 is associated with normal direction.
*/
Vec3f asdf_interior_point_wrt_face(const ASDF* const asdf, 
	const int f, const float w1, const float w2, const float w3)
{
	Vec3f p[8];
	for(int i=0; i<8; ++i){p[i] = asdf_vertex_point(asdf,i);} //generate standard verts
	switch (f){
		case 0: return trilinear_point(p, w3, w1, w2); //-x
		case 1: return trilinear_point(p,-w3, w1, w2); //+x
		case 2: return trilinear_point(p, w1, w3, w2); //-y
		case 3: return trilinear_point(p, w1,-w3, w2); //+y
		case 4: return trilinear_point(p, w1, w2, w3); //-z
		case 5: return trilinear_point(p, w1, w2,-w3); //+z
	}
	return (Vec3f){0,0,0};
}

//likely a duplicate of asdf_interpolate
float asdf_interp_vec(const ASDF* const asdf,
											const Vec3f X)
{
	Vec3f scaled = vdiv(vdiff(X,asdf_min_pt(asdf)),asdf_span(asdf));
	return asdf->d[0]*(1-scaled.x)*(1-scaled.y)*(1-scaled.z)
				+asdf->d[1]*(1-scaled.x)*(1-scaled.y)*(scaled.z)
				+asdf->d[2]*(1-scaled.x)*(scaled.y)*(1-scaled.z)
				+asdf->d[3]*(1-scaled.x)*(scaled.y)*(scaled.z)
				+asdf->d[4]*(scaled.x)*(1-scaled.y)*(1-scaled.z)
				+asdf->d[5]*(scaled.x)*(1-scaled.y)*(scaled.z)
				+asdf->d[6]*(scaled.x)*(scaled.y)*(1-scaled.z)
				+asdf->d[7]*(scaled.x)*(scaled.y)*(scaled.z);
}

float asdf_ray_zero_crossing(const ASDF* const leaf,
                         const Vec3f b, const Vec3f v)
{
    float d = 1.;
    assert(asdf_interp_vec(leaf,b)<0);
    
    //increase until b+d*v is positive
    for (int iteration = 0; iteration < 16; ++iteration) {
    	const Vec3f pos = vsum(b,vscale(d,v));
    	const float result = asdf_interp_vec(leaf,pos);
			if (result < 0)				d *= 2;
			else									{assert(result>0); break;} 	
    }

    d = d/2; //start at midpoint
    float step = 0.5*d;
    for (int iteration = 0; iteration < 16; ++iteration) {
    	const Vec3f pos = vsum(b,vscale(d,v));
    	const float result = asdf_interp_vec(leaf,pos);
    	if (result < -EPSILON)         d += step;
    	else if (result > EPSILON)     d -= step;
    	else                           break;
      step /= 2;
    }
    return d;
}



/*
Starting from edge in asdf, calculate the ray with canonical
angle given by gaussian weight w.
The magnitude is scaled so vector just spans cell from edge.
*/
Vec3f adsf_internal_ray_wrt_edge(const ASDF* const asdf, const int e, const float w)
{
	const int v0 = EDGES[e][0];
	const int v1 = EDGES[e][1];
	int v2,v3;
	if(v1==(v0^1)){
		v2 = v0^2; v3 = v0^4;
	} else if (v1==(v0^2)){
		v2 = v0^1; v3 = v0^4;
	} else if (v1==(v0^4)){
		v2 = v0^1; v3 = v0^2;
	} else{
		assert(0); //something is wrong
	}
	Vec3f d2 = vdiff(asdf_vertex_point(asdf,v2),asdf_vertex_point(asdf,v0));
	Vec3f d3 = vdiff(asdf_vertex_point(asdf,v3),asdf_vertex_point(asdf,v0));
	float th = gauss_to_interval(w,(Interval){0,M_PI/2.});
	Vec3f V = vsum( vscale(cos(th),d2), vscale(sin(th),d3) );
	if (th==0 || th==M_PI/2.){
		return V;
	} else {
		//scale so touches cell boundary
		const float dd2 = asdf_vertex_distance(asdf,v0,v2);
		const float dd3 = asdf_vertex_distance(asdf,v0,v3);
		const float scale = fmin( dd2/dot(V,d2), dd3/dot(V,d3));
		//printf("scale=%f\n",scale);
		return vscale( scale, V);
	}
}

Vec3f asdf_internal_ray_wrt_vertex(const ASDF* const asdf, const int v, const float w1, const float w2)
{
	const int v1 = v^4;
	const int v2 = v^2;
	const int v3 = v^1;

	Vec3f d1 = vdiff(asdf_vertex_point(asdf,v1),asdf_vertex_point(asdf,v));
	Vec3f d2 = vdiff(asdf_vertex_point(asdf,v2),asdf_vertex_point(asdf,v));
	Vec3f d3 = vdiff(asdf_vertex_point(asdf,v3),asdf_vertex_point(asdf,v));
	float th = gauss_to_interval(w1,(Interval){0,M_PI/2.});
	float ph = gauss_to_interval(w2,(Interval){0,M_PI/2.});
	
	Vec3f V = vsum(
							vsum( 
								vscale(cos(th)*sin(ph),d1), 
								vscale(sin(th)*sin(ph),d2)
							),
							vscale(cos(ph),d3)
						);
	
	if ( (th==0 || th==M_PI/2.)&&(ph==0 || ph==M_PI/2.) ){
		return V;
	} else {
		//scale so touches cell boundary
		const float dd1 = asdf_vertex_distance(asdf,v,v1);
		const float dd2 = asdf_vertex_distance(asdf,v,v2);
		const float dd3 = asdf_vertex_distance(asdf,v,v3);
		const float scale = fmin( dd1/dot(V,d1), fmin( dd2/dot(V,d2), dd3/dot(V,d3)));
		//printf("scale=%f\n",scale);
		return vscale( scale, V);
	}
}


Vec3f asdf_interior_point_wrt_edge(const ASDF* asdf,const int e, const float w1, const float w2, const float w3)
{
	Vec3f base = linear_point(asdf_vertex_point(asdf,EDGES[e][0]), asdf_vertex_point(asdf,EDGES[e][1]), w1);
	Vec3f angled = adsf_internal_ray_wrt_edge(asdf,e,w2);
	return vsum( base, linear_point( (Vec3f){0,0,0}, angled, w3));
}

Vec3f asdf_interior_point_wrt_vertex(const ASDF* asdf,const int v, const float w1, const float w2, const float w3){
	Vec3f base = asdf_vertex_point(asdf,v);
	Vec3f angled = asdf_internal_ray_wrt_vertex(asdf,v,w1,w2);
	return vsum( base, linear_point( (Vec3f){0,0,0}, angled, w3));
}


/**
@brief attempt to find a face contained in the interior, if no face, return -1
*/
int asdf_find_interior_face(const ASDF* const asdf)
{
	const float* d = asdf->d;
	for(int f=0; f<6; ++f){
		if (d[FACES[f][0]]<0 && d[FACES[f][1]]<0 && d[FACES[f][2]]<0 && d[FACES[f][3]]<0){
			return f;
		}
	}
	return -1; //didn't find an interior face
}
/**
@brief attempt to find a edge contained in the interior, if no edge, return -1
*/
int asdf_find_interior_edge(const ASDF* const asdf)
{
	const float* d = asdf->d;
	for(int e=0; e<12; ++e){
		if (d[EDGES[e][0]]<0 && d[EDGES[e][1]]<0){
			return e;
		}
	}
	return -1; //didn't find an interior edge
}
/**
@brief attempt to find a vertex contained in the interior, if no vertex, return -1
*/
int asdf_find_interior_vertex(const ASDF* const asdf)
{
	const float* d = asdf->d;
	for(int v=0; v<8; ++v){
		if (d[v]<0){return v;}
	}
	return -1; //didn't find an interior edge
}


int asdf_integrate_cartesian(const ASDF* const asdf, const ASDF* const boundary,
	const int N, IntegrationPoint* const result)
{
	assert(asdf->state == FILLED);
	int n = 0; //keep track of current point number
	for(int i=0; i<N; ++i){
		for(int j=0; j<N; ++j){
			for(int k=0; k<N; ++k){
				n = i*N*N + j*N + k;
				const Vec3f pt = asdf_interior_point(asdf,gauss_pt(i,N),gauss_pt(j,N),gauss_pt(k,N));
				const float weight = (1/8.)*asdf_volume(asdf)*gauss_wt(i,N)*gauss_wt(j,N)*gauss_wt(k,N);
				result[n] = (IntegrationPoint){
							pt,weight,
							asdf_sample_monotone(boundary,pt),
							asdf_sample_gradient_monotone(boundary,pt)};
			}
		}
	}
	return N*N*N;
}


int asdf_integrate_wrt_face(const ASDF* const leaf, const ASDF* const boundary,
	const int N, IntegrationPoint* const result, const int f)
{
	//for each point on contained face, find intersection of normal ray
	float s1, s2;
	for(int i=0; i<N; ++i){
		for(int j=0; j<N; ++j){
			s1 = gauss_pt(i,N); s2 = gauss_pt(j,N);
			const Vec3f b = asdf_interior_point_wrt_face(leaf,f,s1,s2,-1);
			const Vec3f v = FACE_NORMAL_VEC[f];

			float d = asdf_ray_zero_crossing(leaf,b,v);
			assert(d>=0);
			const float cd = asdf_cell_dimension(leaf,FACE_NORMAL[f]);
			d = fmin(cd,d); //distance to intersection or cell's opposing face

			for(int k=0; k<N; ++k){
				Interval interv = (Interval){-1,-1+d/cd};
				float s3 = gauss_to_interval(gauss_pt(k,N),interv);
				const int n = i*N*N + j*N + k;
				const Vec3f pt = asdf_interior_point_wrt_face(leaf,f,s1,s2,s3);
				const float weight = asdf_face_area(leaf,f)*(d/2)*(1/8.)*gauss_wt(i,N)*gauss_wt(j,N)*gauss_wt(k,N);
				result[n] = (IntegrationPoint){
							pt,weight,
							asdf_sample_monotone(boundary,pt),
							asdf_sample_gradient_monotone(boundary,pt)};
			}
		}
	}
	return N*N*N;
}

/*
Assume no face of leaf is contained, but an edge e is.
Q: do we need to treat the case of more than one edge contained?
	For now, assume ASDF is refined enough to ignore these cases.
*/
int asdf_integrate_wrt_edge(const ASDF* const leaf, const ASDF* const boundary,
	const int N, IntegrationPoint* const result, const int e)
{
	//for each point on contained edge, and angles in [0,pi/2],
	// find intersection of ray with distance zero
	float s1, s2;
	for(int i=0; i<N; ++i){
		for(int j=0; j<N; ++j){
			s1 = gauss_pt(i,N); s2 = gauss_pt(j,N);
			const Vec3f b = asdf_interior_point_wrt_edge(leaf,e,s1,-1,-1);
			const Vec3f v = adsf_internal_ray_wrt_edge(leaf,e,s2);
			
			float d = asdf_ray_zero_crossing(leaf,b,v);
			assert(d>=0);

			d = fmin(1.,d)*norm(v); //distance to intersection or cell's opposing face

			for(int k=0; k<N; ++k){
				Interval interv = (Interval){-1,-1+d/norm(v)};
				float s3 = gauss_to_interval(gauss_pt(k,N),interv);
				const int n = i*N*N + j*N + k;
				const Vec3f pt = asdf_interior_point_wrt_edge(leaf,e,s1,s2,s3);
				const float weight = (1/8.)*asdf_edge_length(leaf,e)*(M_PI/2)*(d/2.)*(d/2)*gauss_wt(i,N)*gauss_wt(j,N)*gauss_wt(k,N);;
				result[n] = (IntegrationPoint){
							pt,weight,
							asdf_sample_monotone(boundary,pt),
							asdf_sample_gradient_monotone(boundary,pt)};
			}
		}
	}
	return N*N*N;
}


/*
Assume no face nor edge of leaf is contained, but a vertex v is.
Q: do we need to treat the case of more than one vertex contained?
	For now, assume ASDF is refined enough to ignore these cases.
*/
int asdf_integrate_wrt_vertex(const ASDF* const leaf, const ASDF* const boundary,
	const int N, IntegrationPoint* const result, const int vert)
{
		//for each pair of angles in [0,pi/2]x[0,pi/2],
	// find intersection of ray with distance zero
	float s1, s2;
	for(int i=0; i<N; ++i){
		for(int j=0; j<N; ++j){
			s1 = gauss_pt(i,N); s2 = gauss_pt(j,N);
			const Vec3f b = asdf_interior_point_wrt_vertex(leaf,vert,-1,-1,-1);
			const Vec3f v = asdf_internal_ray_wrt_vertex(leaf,vert,s1,s2);
			
			float d = asdf_ray_zero_crossing(leaf,b,v);
			assert(d>=0);

			d = fmin(1.,d)*norm(v); //distance to intersection or cell's opposing face
			float ph = gauss_to_interval(s2,(Interval){0,M_PI/2.});
			for(int k=0; k<N; ++k){
				Interval interv = (Interval){-1,-1+d/norm(v)};
				float s3 = gauss_to_interval(gauss_pt(k,N),interv);
				const int n = i*N*N + j*N + k;
				const Vec3f pt = asdf_interior_point_wrt_vertex(leaf,vert,s1,s2,s3);
				const float weight = (M_PI/2)*(M_PI/2)*(1/8.)*(sin(ph)*d/2.)*(d/2.)*(d/2)*gauss_wt(i,N)*gauss_wt(j,N)*gauss_wt(k,N);
				//const float weight = (M_PI)*(M_PI/2)*(1/8.)*(sin(ph))*(d/2.)*(d/2)*gauss_wt(i,N)*gauss_wt(j,N)*gauss_wt(k,N);
				//printf("sin=%f, d=%f, weight=%f\n",sin(ph),d,weight);
				result[n] = (IntegrationPoint){
							pt,weight,
							asdf_sample_monotone(boundary,pt),
							asdf_sample_gradient_monotone(boundary,pt)};
			}
		}
	}
	return N*N*N;
}







/**
@brief Create a list of integration points, integration weights, distance values, distance gradient values
@details Return the number of entries in the result
*/
int asdf_integration_points_weights(
	const ASDF* const asdf, const ASDF* const boundary, 
	const int N,
	IntegrationPoint* const result)
{
	int n = 0;
	if(asdf->state == FILLED){
		n = asdf_integrate_cartesian(asdf,boundary,N,result); 
	} else if(asdf->state == LEAF){
		int f = asdf_find_interior_face(asdf);
		if (f!=-1){
			n=asdf_integrate_wrt_face(asdf,boundary,N,result,f);
		} else {
			int e = asdf_find_interior_edge(asdf);
			if (e!=-1 /*&& 0*/){
				n=asdf_integrate_wrt_edge(asdf,boundary,N,result,e);
			} else {
				int v = asdf_find_interior_vertex(asdf);
				if (v!=-1 /*&& 0*/){
					n=asdf_integrate_wrt_vertex(asdf,boundary,N,result,v);
				}
			}
		}
	}
	return n; //return number of integration points in array
}


float asdf_integrate_unity(const ASDF* const asdf, const int N)
{
	float integral = 0;
	int n_pts = 0;
	if (asdf == NULL){
		return integral;
	} else if (asdf->state == BRANCH){
		for(int i=0; i<8; ++i){
			integral = integral + asdf_integrate_unity(asdf->branches[i],N);
		}
	} else if (asdf->state == EMPTY){
	} else{
		IntegrationPoint intp[N*N*N];
		if (asdf->state == FILLED || asdf->state == LEAF){
			n_pts = asdf_integration_points_weights(asdf,asdf,N,intp); //for this simple test, just use the asdf itself as the boundary
		}
		for( int i=0; i<n_pts; ++i){
			integral = integral + intp[i].w; //add volume if filled or leaf
		}
	}
	return integral;
}

int asdf_construct_integration_filled(const ASDF* const asdf, const int N, float* const result, int num_pts)
{
	int new_pts = 0;
	if (asdf == NULL)	return num_pts;
	if (asdf->state == FILLED){
		IntegrationPoint intp[N*N*N];
		new_pts = asdf_integration_points_weights(asdf,asdf,N,intp);
		for( int i=0; i<N*N*N; ++i){
			//printf("%f, %f, %f \n ",intw[8*i+0],intw[8*i+1],intw[8*i+2]);
			result[3*(i+num_pts)+0] = intp[i].p.x;
			result[3*(i+num_pts)+1] = intp[i].p.y;
			result[3*(i+num_pts)+2] = intp[i].p.z;
		}
		num_pts = num_pts + new_pts;
	}
	if (asdf->state == BRANCH){
		for(int i=0; i<8; ++i){
			num_pts = asdf_construct_integration_filled(asdf->branches[i],N, result, num_pts);
		}
	}
	return num_pts;
}

int asdf_construct_integration_leaf(const ASDF* const asdf, const int N, float* const result, int num_pts, int depth, int desired_depth)
{
	int new_pts = 0;
	if (asdf == NULL)	return num_pts;
	//if (asdf->state == LEAF && depth==desired_depth){
	if (asdf->state == LEAF){
		IntegrationPoint intp[N*N*N];
		new_pts = asdf_integration_points_weights(asdf,asdf,N,intp);
		for( int i=0; i<N*N*N; ++i){
			result[3*(i+num_pts)+0] = intp[8*i].p.x;
			result[3*(i+num_pts)+1] = intp[8*i].p.y;
			result[3*(i+num_pts)+2] = intp[8*i].p.z;
		}
		num_pts = num_pts + new_pts;
	}
	if (asdf->state == BRANCH){
		for(int i=0; i<8; ++i){
			num_pts = asdf_construct_integration_leaf(asdf->branches[i],N, result, num_pts, depth+1, desired_depth);
		}
	}
	return num_pts;
}


//so dirty
int write_integration_points(const ASDF* const asdf, const int N, const int desired_depth)
{
	float result[2000000];
	int num_pts = 0;
	num_pts = asdf_construct_integration_filled(asdf,N,result,num_pts);
	printf("Number of points to write:%d\n",num_pts);
	FILE *file; 
	file = fopen("/Users/calischs/mit/SP14/864.14/students/Calisch_Sam/final/asdf/int_pts_test_filled","w");
	if (file == NULL)
	{
    printf("Error opening file!\n");
    exit(1);
	}
	for(int i=0; i<num_pts; ++i){
		fprintf(file,"%f,%f,%f\n",result[3*i],result[3*i+1],result[3*i+2]);
	}
	fclose(file);
	
	num_pts = 0;
	int depth = 0;
	num_pts = asdf_construct_integration_leaf(asdf,N,result,num_pts,depth,desired_depth);
	printf("Number of points to write:%d\n",num_pts);
	//FILE *file; 
	file = fopen("/Users/calischs/mit/SP14/864.14/students/Calisch_Sam/final/asdf/int_pts_test_leaf","w");
	if (file == NULL)
	{
    printf("Error opening file!\n");
    exit(1);
	}
	for(int i=0; i<num_pts; ++i){
		fprintf(file,"%f,%f,%f\n",result[3*i],result[3*i+1],result[3*i+2]);
	}
	fclose(file);
	
	return 0; 
}


/*
OK, let's do integrate bodyforce again, without any shape function heirarchy.
This time, the shape function will just be specified by 
X0: a center point
h: a grid size
*/
Vec3f integrate_bodyforce_bspline(const ASDF* const asdf, const ASDF* const boundary, 
 int N, const Vec3f F, const Vec3f X0, const float h)
{
	const int n = 2; //Use quadratic B-splines
	if (asdf == NULL) return (Vec3f){0,0,0};
	if (asdf->state == EMPTY) return (Vec3f){0,0,0};

	//assert(does_contain(asdf_box(asdf),bspline_support(n,X0,h)));
	assert(is_valid_box(box_intersection(asdf_box(asdf),bspline_support(n,X0,h))));
	//Note, as long as the shape function isn't on the edge of the domain, we should have 
	//containment instead of just overlap.
	if (asdf->state == BRANCH){
		//if branch, need to first home in on the support of bspline
		for(int i=0; i<8; ++i){
			if(asdf->branches[i] != NULL){
				if (does_contain(asdf_box(asdf->branches[i]),bspline_support(n,X0,h))){
					return integrate_bodyforce_bspline(asdf->branches[i],boundary,N,F,X0,h);
				}
			}
		}
	}
	//OK, we are now either in FILLED/LEAF, or the smallest branch completely containing the support
	// In this region, we let the zero of the function adapt to the boundary.
	assert(asdf != NULL);
	return integrate_bodyforce_bspline_focused(asdf,boundary,N,F,X0,h,n);
}

Vec3f integrate_bodyforce_bspline_focused(const ASDF* const asdf, const ASDF* const boundary, 
 int N, const Vec3f F, const Vec3f X0, const float h, const int n)
{
	Vec3f integral = (Vec3f){0,0,0}; //vector valued
	int n_pts = 0;
	assert(asdf != NULL);
	if (asdf->state == BRANCH){
		for(int i=0; i<8; ++i){
			if(asdf->branches[i] != NULL){
				integral = vsum(integral, integrate_bodyforce_bspline_focused(asdf->branches[i],boundary,N,F,X0,h,n));
			}
		}
	} else{
		IntegrationPoint intp[N*N*N];
		if (asdf->state == FILLED || asdf->state == LEAF){
			n_pts = asdf_integration_points_weights(asdf,boundary,N,intp);  
			for( int i=0; i<n_pts; ++i){
				const Vec3f val = vscale(-intp[i].d * bspline_3(intp[i].p,n,X0,h), F );
				Vec3f d_int = vscale(intp[i].w, val ); //multiply by integration weight
				integral = vsum(integral, d_int);  //accumulate
			}
		}
	}
	return integral;
}


/*
Take two spline descriptions to generate stiffness submatrix
Also, take Lame coefficients
First check if their support boxes intersect.  If no, return 0.
Then, home in on support intersection box.
Return a 3x3 matrix of shape function interactions.
The matrix expression can be generated with the sympy:
#mu = E/(2*(1+nu))
#l = E*nu/(1+nu)/(1-2*nu)
ui,uj = symbols('u_i,u_j') #shape functions (assumes same in each coordinate dim)
dxui,dyui,dzui = symbols('u^i_x u^i_y u^i_z') #di
dxuj,dyuj,dzuj = symbols('u^j_x u^j_y u^j_z') #dj
mu,l = symbols('mu l') #Lame coeff
Bui = Matrix([[dxui,0,0],[0,dyui,0],[0,0,dzui],[dyui,dxui,0],[0,dzui,dyui],[dzui,0,dxui]])
Buj = Matrix([[dxuj,0,0],[0,dyuj,0],[0,0,dzuj],[dyuj,dxuj,0],[0,dzuj,dyuj],[dzuj,0,dxuj]])
D = Matrix([
    [2*mu+l,l,l,0,0,0],
    [l,2*mu+l,l,0,0,0],
    [l,l,2*mu+l,0,0,0],
    [0,0,0,     2*mu,0,0],
    [0,0,0,     0,2*mu,0],
    [0,0,0,     0,0,2*mu]
    ])
#Buj.T*(D*Bui)
ccode(Bu.T.dot(D.dot(Bu)))
*/
Mat3f integrate_stiffness_bspline(const ASDF* const asdf, const ASDF* const boundary,
 int N, const Vec3f X0, const Vec3f X1, const float h, 
 const float mu, const float l)
{
	const int n = 2; //Use quadratic B-splines
	Box intersection = box_intersection(bspline_support(n,X0,h),bspline_support(n,X1,h)); 
	if ( is_valid_box(intersection)==0 ){
		//printf("bspline supports don't overlap");
		return (Mat3f){0,0,0,0,0,0,0,0,0};
	}

	if (asdf == NULL) return (Mat3f){0,0,0,0,0,0,0,0,0};
	if (asdf->state == EMPTY) return (Mat3f){0,0,0,0,0,0,0,0,0};
	//assert(does_contain(asdf_box(asdf),intersection));
	assert(is_valid_box(box_intersection(asdf_box(asdf),intersection)));

	if (asdf->state == BRANCH){
		//if branch, need to first home in on the intersection of support of bsplines
		for(int i=0; i<8; ++i){
			if (asdf->branches[i] != NULL){
				if (does_contain(asdf_box(asdf->branches[i]),intersection)){
					//printf("found containing branch");
					return integrate_stiffness_bspline(asdf->branches[i],boundary,N,X0,X1,h,mu,l);
				}
			}
		}
	}
	//OK, we are now either in FILLED/LEAF, or the smallest branch containing the support
	// In this region, we let the zero of the function adapt to the boundary.
	return integrate_stiffness_bspline_focused(asdf,boundary,N,X0,X1,h,mu,l,n);
}

Mat3f integrate_stiffness_bspline_focused(const ASDF* const asdf, const ASDF* const boundary,
 int N, const Vec3f X0, const Vec3f X1, const float h, 
 const float mu, const float l, const int n)
{
	Mat3f integral = (Mat3f){0,0,0,0,0,0,0,0,0}; //matrix valued
	int n_pts = 0;
	if (asdf->state == BRANCH){
		for(int i=0; i<8; ++i){
			if( asdf->branches[i] != NULL ){
				integral = msum(integral, integrate_stiffness_bspline_focused(asdf->branches[i],boundary,N,X0,X1,h,mu,l,n));
			}
		}
	} else{
		IntegrationPoint intp[N*N*N];
		if (asdf->state == FILLED || asdf->state == LEAF){
			n_pts = asdf_integration_points_weights(asdf,boundary,N,intp);  
			for( int i=0; i<n_pts; ++i){
				//compute the incremental matrix
				const float chi_i = bspline_3(intp[i].p,n,X0,h); //unscaled shape func
				const float chi_j = bspline_3(intp[i].p,n,X1,h); //unscaled shape func
				//chain rule on distance scaled shape functions
				const Vec3f di = vsum(vscale(intp[i].d,grad_bspline_3(intp[i].p,n,X0,h)), vscale(chi_i,intp[i].grad));
				const Vec3f dj = vsum(vscale(intp[i].d,grad_bspline_3(intp[i].p,n,X1,h)), vscale(chi_j,intp[i].grad));
				const float diag = dot(di,dj);
				const Mat3f val = mscale(intp[i].w,
						(Mat3f){
							2*mu*diag + l*di.x*dj.x, l*di.y*dj.x+2*mu*di.x*dj.y, l*di.z*dj.x+2*mu*di.x*dj.z,
							l*di.x*dj.y + 2*mu*di.y*dj.x, 2*mu*diag+l*di.y*dj.y, l*di.z*dj.y+2*mu*di.y*dj.z,
							l*di.x*dj.z + 2*mu*di.z*dj.x, l*di.y*dj.z+2*mu*di.z*dj.y, 2*mu*diag+l*di.z*dj.z
						}
					);
				integral = msum(integral, val);  //accumulate
			}
		}
	}
	return integral;
}

/*
Next is an integration routine for a set of non-zero essential boundary conditions
u^* is a vector valued function that transfinitely interpolates all nonzero displacements over the whole domain.
For the fixed displacements, the assumed solution structure enforces these constraints.

As triage, let's do this after implementing the problem setup.
*/

















/*
Take in a triple of Intervals specifying a quadratic b-spline
//Integrate unity times this shape function recursively
F is the body force -- for now, just a constant vector
N is the degree of gaussian quadrature rule to use.
*/
/*
Vec3f integrate_bodyforce_bspline(const ASDF* const asdf, const Vec3f F,
 int N, const Interval Ix,const Interval Iy,const Interval Iz)
{
	Vec3f integral = (Vec3f){0,0,0};
	
	int n_pts = 0;
	if (asdf == NULL){
		return integral;
	} else if (asdf->state == BRANCH){
		for(int i=0; i<8; ++i){
			integral = vsum(integral, integrate_body_bspline(asdf->branches[i],N,Ix,Iy,Iz));
		}
	} else if (asdf->state == EMPTY){
	} else{
		float intw[8*N*N*N];
		if (asdf->state == FILLED || asdf->state == LEAF){
			n_pts = asdf_integration_points_weights(asdf,N,intw);  
			for( int i=0; i<n_pts; ++i){
				//add value if filled or leaf
				const Vec3f pt = (Vec3f){intw[8*i],intw[8*i+1],intw[8*i+2]};
				const Vec3f val = vmult(b_spline_3k(pt,2,Ix,Iy,Iz),F); //cheating dot product
				Vec3f d_int = vmult(intw[8*i+3], val );
				integral = vsum(integral, d_int); 
			}
		}
	}
	return integral;

	//assert(asdf->state==BRANCH)
}
*/

/*
Place a quadratic b-spline shape function across the entire ASDF
Then call the b-spline integration routine
For each cell being used to generate a shape function, call this function
Body force is given by a constant vector F, for now.
*/
/*
Vec3f integrate_bodyforce(const ASDF* const asdf, const Vec3f F, const int N)
{
	return integrate_bodyforce_bspline(asdf,F,N,asdf->X,asdf->Y,asdf->Z);
}


*/


/*
Take in a triple of Intervals specifying a quadratic b-spline
//Integrate strain energy recursively

*/
/*
Vec3f integrate_stiffness_bspline(
	const ASDF* const asdf_i, 
	const ASDF* const asdf_j, 
 const float mu, const float l,
 int N, const Interval Ix,const Interval Iy,const Interval Iz)
{

	//First, check intersection of asdf_i and asdf_j.  If don't intersect, return 0
	//

	Vec3f integral = (Vec3f){0,0,0};

	int n_pts = 0;
	if (asdf == NULL){
		return integral;
	} else if (asdf->state == BRANCH){
		for(int i=0; i<8; ++i){
			integral = vsum(integral, integrate_body_bspline(asdf->branches[i],N,Ix,Iy,Iz));
		}
	} else if (asdf->state == EMPTY){
	} else{
		float intw[8*N*N*N];
		if (asdf->state == FILLED || asdf->state == LEAF){
			n_pts = asdf_integration_points_weights(asdf,N,intw);  
			for( int i=0; i<n_pts; ++i){
				const Vec3f pt = (Vec3f){intw[8*i],intw[8*i+1],intw[8*i+2]};




				const Vec3f val = vmult(b_spline_3k(pt,2,Ix,Iy,Iz),F);
				Vec3f d_int = vmult(intw[8*i+3], val );
				integral = vsum(integral, d_int); 
			}
		}
	}
	return integral;

	//assert(asdf->state==BRANCH)
}
*/

/*
Place a quadratic b-spline shape function across the entire ASDF
Then call the b-spline integration routine
For each cell being used to generate a shape function, call this function
mu,l are Lame coefficients
N is the degree of gaussian quadrature rule to use.
*/

/*
Vec3f integrate_stiffness(const ASDF* const asdf, const float mu, const float l, const int N)
{
	return integrate_stiffness_bspline(asdf,mu,l,N,asdf->X,asdf->Y,asdf->Z);
}

*/