// heightfield.cc*
// Source Code Copyright
/*
 * lrt source code Copyright(c) 1998-2002 Matt Pharr and Greg Humphreys
 * All Rights Reserved.
 * For non-commercial use only.
 * No warranty, express or implied, for this software.
 */
#include "lrt.h"
#include "shapes.h"
#include "paramset.h"
#include "dynload.h"
#include "trianglemesh.h"
// Heightfield Declarations
// *** rui
// use refine : return false at function CanIntersect()
// #define REFINE_METHOD

class Heightfield : public Shape {
public:
	// Heightfield Method Declarations
	Heightfield(const Transform &o2w, int nu, int nv, const Float *zs);
	~Heightfield();
	bool CanIntersect() const;
	// *** rui
	bool Intersect(const Ray &ray, Float *thitp, DifferentialGeometry *dg) const;
	bool IntersectP(const Ray &ray) const;
	void Refine(vector<RefinedShape> &refined) const;
	BBox ObjectBound() const;
private:
	// Heightfield Data
	Float *z;
	int nx, ny;
	// *** rui
	BBox BB;
	BBox *bb;
	Point *point;
	int mx, my;
	Vector *normal;
	void create_normal();
	void create_bbox();
	void error(char *msg) { printf("Error: %s.\n", msg); }
	bool TriangleIntersect(const Ray &ray, Point *p, Vector *n,	Float *thitp, DifferentialGeometry *dg) const;
	bool TriangleIntersectP(const Ray &ray, Point *p) const;
	
};

Heightfield::Heightfield(const Transform &o2w, int x, int y,
		const Float *zs)
	: Shape(o2w) {
	nx = x;
	ny = y;
	z = new Float[nx*ny];
	memcpy(z, zs, nx*ny*sizeof(Float));
	// *** rui
	mx = nx-1;
	my = ny-1;
	point = new Point[nx*ny];
	normal = new Vector[nx*ny];
	create_normal();
	bb = new BBox[mx*my];
	create_bbox();
}

Heightfield::~Heightfield() {
	delete[] z;
	delete[] normal;
	delete[] bb;
	delete[] point;
}

// *** rui
#define mv(x, y)   ((x)+(y)*mx)
#define nv(x, y)   ((x)+(y)*nx)

void Heightfield::create_normal()
{
	int x, y;
	Point *P = point;
    Point *WP = new Point[nx*ny];
	int pos = 0;
	for (y = 0; y < ny; ++y) {
		for (x = 0; x < nx; ++x) {
			P[pos].x = (Float)x / (Float)mx;
			P[pos].y = (Float)y / (Float)my;
			P[pos].z = z[pos];
            WP[pos] = ObjectToWorld(P[pos]);
            normal[pos] = Vector(0,0,0);
			++pos;
		}
	}	

	Vector v1, v0;
	Vector n;
	int idx0, idx1, idx2, idx3;

    // calculate vertex normal
	for( y=0; y<my; y++)
		for( x=0; x<mx; x++)
		{
			idx0 = nv(x, y);
			idx1 = idx0 + 1;
			idx2 = nv(x+1, y+1);
			idx3 = idx2 - 1;
            v0 = WP[idx1] - WP[idx0];
			v1 = WP[idx2] - WP[idx0];
			n = Cross(v0, v1);
			normal[idx0] += n;
			normal[idx1] += n;
			normal[idx2] += n;
            v0 = WP[idx2] - WP[idx0];
            v1 = WP[idx3] - WP[idx0];
			n = Cross(v0, v1);
			normal[idx2] += n;
			normal[idx3] += n;
			normal[idx0] += n;
		}
	for( x=0; x<nx*ny; x++)
		normal[x] = normal[x].Hat();
    delete[] WP;
}

void Heightfield::create_bbox()
{
	BB = ObjectBound();
	int x, y;
	Float minz, maxz;
	int pos = 0;
	int idx0, idx1, idx2, idx3;
	for(y=0; y<my ; y++)
		for(x=0 ; x<mx; x++, pos++) {
			idx0 = nv(x, y);
			idx1 = idx0 + 1;
			idx2 = nv(x+1, y+1);
			idx3 = idx2 - 1;			
			minz = min(z[idx0], min(z[idx1], min(z[idx2], z[idx3])));
			maxz = max(z[idx0], max(z[idx1], max(z[idx2], z[idx3])));
			bb[pos] = BBox(Point((Float)x / (Float)mx, (Float)y / (Float)my, minz), 
				         Point((Float)(x+1) / (Float)mx, (Float)(y+1) / (Float)my, maxz));
	}
}
	
BBox Heightfield::ObjectBound() const {
	Float minz = z[0], maxz = z[0];
	for (int i = 1; i < nx*ny; ++i) {
		if (z[i] < minz) minz = z[i];
		if (z[i] > maxz) maxz = z[i];
	}
	return BBox(Point(0,0,minz), Point(1,1,maxz));
}

bool Heightfield::CanIntersect() const {
	// *** rui
	#ifdef REFINE_METHOD
		return false;
	#else
		return true;
	#endif
}

void Heightfield::Refine(vector<RefinedShape> &refined) const {
	int ntris = 2*(nx-1)*(ny-1);
	refined.reserve(ntris);
	int *verts = new int[3*ntris];
	Point *P = new Point[nx*ny];
	Float *uvs = new Float[2*nx*ny];
	int nverts = nx*ny;
	int x, y;
	// Compute heightfield vertex positions
	P = new Point[nx*ny];
	int pos = 0;
	for (y = 0; y < ny; ++y) {
		for (x = 0; x < nx; ++x) {
			P[pos].x = uvs[2*pos]   = (Float)x / (Float)(nx-1);
			P[pos].y = uvs[2*pos+1] = (Float)y / (Float)(ny-1);
			P[pos].z = z[pos];
			++pos;
		}
	}
	// Fill in heightfield vertex offset array
	int *vp = verts;
	for (y = 0; y < ny-1; ++y) {
		for (x = 0; x < nx-1; ++x) {
	#define VERT(x,y) ((x)+(y)*nx)
			*vp++ = VERT(x, y);
			*vp++ = VERT(x+1, y);
			*vp++ = VERT(x+1, y+1);
	
			*vp++ = VERT(x, y);
			*vp++ = VERT(x+1, y+1);
			*vp++ = VERT(x, y+1);
		}
	#undef VERT
	}
	ParamSet paramSet(nx*ny);
	paramSet.addInt("_ntris", &ntris);
	paramSet.addInt("_nverts", &nverts);
	paramSet.addInt("_vertexIndices", verts, PARAM_TYPE_UNIFORM,
		3*ntris);
	paramSet.addFloat("uv", uvs, PARAM_TYPE_VERTEX, 2);
	paramSet.addPoint("P", P, PARAM_TYPE_VERTEX);
	refined.push_back(RefinedShape(
		CreateShape("trianglemesh", LRT_BUILDDIR,
			ObjectToWorld, paramSet)));
	delete[] P;
	delete[] uvs;
	delete[] verts;
}

extern "C" Reference<Shape>CreateShape(const Transform &o2w,
		const ParamSet &params) {
	int nu = params.findOneInt("_nu", -1);
	int nv = params.findOneInt("_nv", -1);
	int type, narray;
	const Float *Pz = params.findFloat("Pz", &type, &narray);
	Assert(nu != -1 && nv != -1 && Pz != NULL);
	return new Heightfield(o2w, nu, nv, Pz);
}

// *** rui
#define SMALL_NUMBER	1.0e-8
#define HUGE_NUMBER	    INFINITY

#define clamp(x, a, b)	((x)<(a)?(a):((x)>(b)?(b):(x)))
#define absf(x)         ((x)>0?(x):-(x))
bool Heightfield::Intersect(const Ray &r, Float *thitp,
		DifferentialGeometry *dg) const {
	Ray ray;
	WorldToObject(r, &ray);			

	Float thit0, thit1, curr_t;
	int X, Y;
	int stepX, stepY;
	Float tMaxX, tMaxY, tDeltaX, tDeltaY;
	
	if( BB.IntersectP(ray, &thit0, &thit1) == false)
		return false;
	
    // initialization
	Point hp0 = ray(thit0);
	X = (int)(hp0.x * mx);
	Y = (int)(hp0.y * my);
	//X = clamp(X, 0, mx-1);
	//Y = clamp(Y, 0, my-1);
	
	if(absf(ray.D.x) < SMALL_NUMBER) {
		stepX = 0;		
		tDeltaX = HUGE_NUMBER;
		tMaxX = HUGE_NUMBER;
	} else {
		if(ray.D.x > 0) {
			stepX = 1;
			tMaxX = ((Float)(X+1) / (Float)mx - hp0.x) / ray.D.x + thit0;
		} else {
			stepX = -1;
			tMaxX = ((Float)X / (Float)mx - hp0.x) / ray.D.x + thit0;
		}
		tDeltaX = (Float) 1 / (Float) mx / absf(ray.D.x);
	}
	
	if(absf(ray.D.y) < SMALL_NUMBER) {
		stepY = 0;
		tDeltaY = HUGE_NUMBER;
		tMaxY = HUGE_NUMBER;
	} else {
		if(ray.D.y > 0) {
			stepY = 1;
			tMaxY = ((Float)(Y+1) / (Float)my - hp0.y) / ray.D.y + thit0;
		} else {
			stepY = -1;
			tMaxY = ((Float)Y / (Float)my - hp0.y) / ray.D.y + thit0;			
		}
		tDeltaY = (Float) 1 / (Float) my / absf(ray.D.y);
	}

	bool hit = false;
	bool hit_tri[2];
	Point p[2][3];	
	Vector n[2][3];
	Float thit_bb0, thit_bb1;
	Float thit_tri[2];
	DifferentialGeometry dg_tri[2];
	int choose;
	int idx0, idx1, idx2, idx3;
	// incremental traversal
	curr_t = thit0;
	do {
		// process current voxel
		if( bb[mv(X, Y)].IntersectP(ray, &thit_bb0, &thit_bb1) == false)
			goto INCREMENTAL_TRAVERSE;
		// intersect with the two triangles and compare the result
		idx0 = nv(X, Y);
		idx1 = idx0 + 1;
		idx2 = nv(X+1, Y+1);
		idx3 = idx2 - 1;
		p[0][0] = point[idx0];
		p[0][1] = point[idx1];
		p[0][2] = point[idx2];
		n[0][0] = normal[idx0];
		n[0][1] = normal[idx1];
		n[0][2] = normal[idx2];
		hit_tri[0] = TriangleIntersect(ray, p[0], n[0], &thit_tri[0], &dg_tri[0]);
		
        p[1][0] = point[idx0];
		p[1][1] = point[idx2];
		p[1][2] = point[idx3];
		n[1][0] = normal[idx0];
		n[1][1] = normal[idx2];
		n[1][2] = normal[idx3];
		hit_tri[1] = TriangleIntersect(ray, p[1], n[1], &thit_tri[1], &dg_tri[1]);		
		
        if(hit_tri[0] == false && hit_tri[1] == false)
			goto INCREMENTAL_TRAVERSE;
		if(hit_tri[0] == true && hit_tri[1] == true)
			choose = (thit_tri[0] < thit_tri[1])?0:1;
		else if(hit_tri[0] == false)
			choose = 1;
		else
			choose = 0;
		*dg = dg_tri[choose];
		*thitp = thit_tri[choose];
		hit = true;
		break;
			
INCREMENTAL_TRAVERSE:
		if(tMaxX < tMaxY) {
			curr_t = tMaxX;
			tMaxX += tDeltaX;
			X += stepX;
		} else {
			curr_t = tMaxY;
			tMaxY += tDeltaY;
			Y += stepY;
		}
	} while(hit == false && curr_t < thit1 && X > -1 && Y > -1 && X < mx && Y < my);
	return hit;
}


bool Heightfield::IntersectP(const Ray &r) const
{
	Ray ray;
	WorldToObject(r, &ray);			

	Float thit0, thit1, curr_t;
	int X, Y;
	int stepX, stepY;
	Float tMaxX, tMaxY, tDeltaX, tDeltaY;
	
	if( BB.IntersectP(ray, &thit0, &thit1) == false)
		return false;

	Point hp0 = ray(thit0);
	X = (int)(hp0.x * mx);
	Y = (int)(hp0.y * my);
	//X = clamp(X, 0, mx-1);
	//Y = clamp(Y, 0, my-1);
	
	if(absf(ray.D.x) < SMALL_NUMBER) {
		stepX = 0;		
		tDeltaX = HUGE_NUMBER;
		tMaxX = HUGE_NUMBER;
	} else {
		if(ray.D.x > 0) {
			stepX = 1;
			tMaxX = ((Float)(X+1) / (Float)mx - hp0.x) / ray.D.x + thit0;
		} else {
			stepX = -1;
			tMaxX = ((Float)X / (Float)mx - hp0.x) / ray.D.x + thit0;
		}
		
		tDeltaX = (Float) 1 / (Float) mx / absf(ray.D.x);
	}
	
	if(absf(ray.D.y) < SMALL_NUMBER) {
		stepY = 0;
		tDeltaY = HUGE_NUMBER;
		tMaxY = HUGE_NUMBER;
	} else {
		if(ray.D.y > 0) {
			stepY = 1;
			tMaxY = ((Float)(Y+1) / (Float)my - hp0.y) / ray.D.y + thit0;
		} else {
			stepY = -1;
			tMaxY = ((Float)Y / (Float)my - hp0.y) / ray.D.y + thit0;			
		}
		tDeltaY = (Float) 1 / (Float) my / absf(ray.D.y);
	}

	
	bool hit = false;
	Float thit_bb0, thit_bb1;
	int idx0, idx1, idx2, idx3;
	Point p[3];
	// incremental traversal
	curr_t = thit0;
	do {
		// process current voxel
		if( bb[mv(X, Y)].IntersectP(ray, &thit_bb0, &thit_bb1) == false)
			goto INCREMENTAL_TRAVERSE;
		// intersect with the two triangles and compare the result
		idx0 = nv(X, Y);
		idx1 = idx0 + 1;
		idx2 = nv(X+1, Y+1);
		idx3 = idx2 - 1;
		p[0] = point[idx0];
		p[1] = point[idx1];
		p[2] = point[idx2];
		if(TriangleIntersectP(ray, p) == true)
			return true;
		p[0] = point[idx0];
		p[1] = point[idx2];
		p[2] = point[idx3];
		if(TriangleIntersectP(ray, p) == true)
			return true;
			
INCREMENTAL_TRAVERSE:			
		if(tMaxX < tMaxY) {
			curr_t = tMaxX;
			tMaxX += tDeltaX;
			X += stepX;
		} else {
			curr_t = tMaxY;
			tMaxY += tDeltaY;
			Y += stepY;
		}
	} while(hit == false && curr_t < thit1 && X > -1 && Y > -1 && X < mx && Y < my);
	return hit;
}

// *** rui
// i intend to use Triangle::Intersec directly
// but i had problem creating a triangle mesh (which must be created to initialize a triangle object
// so i have to copy the code here

bool Heightfield::TriangleIntersect(const Ray &ray, Point *p, Vector *n, Float *thitp, DifferentialGeometry *dg) const {
	// Initialize triangle intersection statistics
	static int triangleTests = 0, triangleHits = 0;
    Point p1 = p[0];
    Point p2 = p[1];
    Point p3 = p[2];
    Vector n1 = n[0];
    Vector n2 = n[1];
    Vector n3 = n[2];
	if (triangleTests == 0)
		StatsRegisterPercentage("Geometry",
			"Triangle Ray Intersections",
			&triangleHits, &triangleTests);
	// Update triangle tests count
	++triangleTests;
	
	Vector E1 = p2 - p1;
	Vector E2 = p3 - p1;
	Vector S_1 = Cross(ray.D, E2);
	Float divisor = Dot(S_1, E1);
	if (divisor == 0.)
		return false;
	Float invDivisor = 1.f / divisor;
	// Compute first barycentric coordinate
	Vector T = ray.O - p1;
	Float b1 = Dot(T, S_1) * invDivisor;
	if (b1 < 0. || b1 > 1.)
		return false;
	// Compute second barycentric coordinate
	Vector S_2 = Cross(T, E1);
	Float b2 = Dot(ray.D, S_2) * invDivisor;
	if (b2 < 0. || b1 + b2 > 1.)
		return false;
	// Compute [[t]] to intersection point
	Float t = Dot(E2, S_2) * invDivisor;
	if (t < ray.mint || t > ray.maxt)
		return false;
	++triangleHits;
	// Fill in [[DifferentialGeometry]] from triangle hit
	// Compute triangle partial derivatives
	Vector dPdu, dPdv;
	Float uvs[3][2];
	// get UVs
	// it just happened that here x, y coincide with texture coordinate
	uvs[0][0] = p1.x;
	uvs[0][1] = p1.y;
	uvs[1][0] = p2.x;
	uvs[1][1] = p2.y;
	uvs[2][0] = p3.x;
	uvs[2][1] = p3.y;
    
	// Compute deltas for triangle partial derivatives
	Float du1 = uvs[1][0] - uvs[0][0];
	Float du2 = uvs[2][0] - uvs[0][0];
	Float dv1 = uvs[1][1] - uvs[0][1];
	Float dv2 = uvs[2][1] - uvs[0][1];
	Float dx1 = p2.x - p1.x;
	Float dx2 = p3.x - p1.x;
	Float dy1 = p2.y - p1.y;
	Float dy2 = p3.y - p1.y;
	Float dz1 = p2.z - p1.z;
	Float dz2 = p3.z - p1.z;
	Float determinant = du1 * dv2 - dv1 * du2;
	if (determinant == 0) {
		// Handle zero determinant for triangle partial derivative matrix
		CoordinateSystem(Cross(E2, E1).Hat(), &dPdu, &dPdv);
	}
	else {
		Float invdet = 1. / determinant;
		dPdu = Vector((dx1 * dv2 - dv1 * dx2) * invdet,
			(dy1 * dv2 - dv1 * dy2) * invdet,
			(dz1 * dv2 - dv1 * dz2) * invdet);
		dPdv = Vector((du1 * dx2 - dx1 * du2) * invdet,
			(du1 * dy2 - dy1 * du2) * invdet,
			(du1 * dz2 - dz1 * du2) * invdet);
	}
	// Interpolate $(u,v)$ triangle parametric coordinates
	Float b0 = 1. - b1 - b2;
	Float tu = b0*uvs[0][0] + b1*uvs[1][0] + b2*uvs[2][0];
	Float tv = b0*uvs[0][1] + b1*uvs[1][1] + b2*uvs[2][1];
	//Float tu = ray(t).x;
    //Float tv = ray(t).y;
    *dg = DifferentialGeometry(ObjectToWorld(ray(t)), ObjectToWorld(dPdu), ObjectToWorld(dPdv), tu, tv, this);
	*thitp = t;
	// *** rui
	Normal n_interp = Normal((b0*n1 + b1*n2 + b2*n3).Hat());
    if(Dot(n_interp, ObjectToWorld(ray.D)) > 0) {
		dg->N = n_interp;
		dg->T = Cross(dg->S, dg->N).Hat();
        dg->S = Cross(dg->N, dg->T).Hat();
    }
	return true;
}
bool Heightfield::TriangleIntersectP(const Ray &ray, Point *p) const {
	// Initialize triangle intersection statistics
    Point p1 = p[0];
    Point p2 = p[1];
    Point p3 = p[2];
	static int triangleTests = 0, triangleHits = 0;
	if (triangleTests == 0)
		StatsRegisterPercentage("Geometry",
			"Triangle Ray Intersections",
			&triangleHits, &triangleTests);
	// Update triangle tests count
	++triangleTests;

	Vector E1 = p2 - p1;
	Vector E2 = p3 - p1;
	Vector S_1 = Cross(ray.D, E2);
	Float divisor = Dot(S_1, E1);
	if (divisor == 0.)
		return false;
	Float invDivisor = 1.f / divisor;
	// Compute first barycentric coordinate
	Vector T = ray.O - p1;
	Float b1 = Dot(T, S_1) * invDivisor;
	if (b1 < 0. || b1 > 1.)
		return false;
	// Compute second barycentric coordinate
	Vector S_2 = Cross(T, E1);
	Float b2 = Dot(ray.D, S_2) * invDivisor;
	if (b2 < 0. || b1 + b2 > 1.)
		return false;
	// Compute [[t]] to intersection point
	Float t = Dot(E2, S_2) * invDivisor;
	if (t < ray.mint || t > ray.maxt)
		return false;
	++triangleHits;
	return true;
}