// 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.
 */

// Modified by John Tran
// Spring 2003


#include "lrt.h"
#include "shapes.h"
#include "paramset.h"
#include "dynload.h"


// Heightfield Declarations
class Heightfield : public Shape 
{
public:
  // Heightfield Method Declarations
  Heightfield(const Transform &o2w, int nu, int nv, const Float *zs);
  ~Heightfield();
  bool CanIntersect() const;
  bool Intersect (const Ray &r, Float *thitp, 
		  DifferentialGeometry *dg) const;
  bool IntersectP (const Ray &ray) const;
  void Refine(vector<RefinedShape> &refined) const;
  BBox ObjectBound() const;
  bool triangleIntersect(const Ray & ray, const Point & p1, 
			 const Point &p2, const Point &p3,
			 const Vector &n1, const Vector &n2, const Vector &n3,
			 Float * thitp, DifferentialGeometry *dg) const;

  bool triangleIntersectP(const Ray & ray, const Point & p1, 
			  const Point &p2, const Point &p3) const;

  int x2v(Float x) const { return Float2Int((x - bbox.pMin.x) * InvXWidth); }
  int y2v(Float y) const { return Float2Int((y - bbox.pMin.y) * InvYWidth); }
  Float v2x(int x) const { return x * XWidth + bbox.pMin.x; }
  Float v2y(int y) const { return y * YWidth + bbox.pMin.y; }
private:
  // Heightfield Data
  Float *z;
  int nx, ny;
  BBox bbox;
  Float XWidth, YWidth;
  Float InvXWidth, InvYWidth;
  int XVoxels, YVoxels;
  Vector * normals;
};

// Heightfield Methods
Heightfield::Heightfield(const Transform &o2w, int xsize, int ysize,
			 const Float *zs)
  : Shape(o2w) 
{
  nx = xsize;
  ny = ysize;
  z = new Float[nx*ny];
  memcpy(z, zs, nx*ny*sizeof(Float));
  

  if (CanIntersect())
    {
      bbox = ObjectBound();
  
      XVoxels = nx - 1;
      YVoxels = ny - 1;

      // Compute voxel widths and allocate voxels
      XWidth = 1. / (Float)XVoxels;
      YWidth = 1. / (Float)YVoxels;
      InvXWidth = 1. /(Float)XWidth;
      InvYWidth = 1. /(Float)YWidth;

      normals = new Vector[nx*ny];
      // do corners first  
      normals[0] = (Cross(Vector(XWidth, 0, z[1] - z[0]), 
			  Vector(0, YWidth, z[nx] - z[0]))).Hat();
      normals[nx-1] = (Cross(Vector(0, YWidth, z[2*nx-1] - z[nx-1]), 
			     Vector(-XWidth, 0, z[nx-2] - z[nx-1]))).Hat();
      normals[(ny-1)*nx] = 
	(Cross(Vector(0, -YWidth, z[(ny-2)*nx] - z[(ny-1)*nx]), 
	       Vector(XWidth, 0, z[(ny-1)*nx+1] - z[(ny-1)*nx]))).Hat();
      normals[(ny-1)*nx+nx-1] = 
	(Cross(Vector(-XWidth, 0, z[(ny-1)*nx+nx-2] - z[(ny-1)*nx+nx-1]), 
	       Vector(0, -YWidth, z[(ny-2)*nx+nx-1] - z[(ny-1)*nx+nx-1]))).Hat();
      for (int y = 0; y < ny; ++y)
	{
	  for (int x = 0; x < nx; ++x)
	    {
	      if ((x==0) && (y==0))
		continue;
	  
	      if (x == 0)
		{
		  Vector up = Vector(0, YWidth, z[x+(y+1)*nx] - z[x+y*nx]);
		  Vector down = Vector (0, -YWidth, z[x+(y-1)*nx] - z[x+y*nx]);
		  Vector right = Vector (XWidth, 0, z[x+1+y*nx] - z[x+y*nx]);
		  if ((y != 0) && (y!=ny-1))
		    normals[0 + y*nx] = (Cross(down, right) 
					 + Cross (right, up)).Hat();
		}
	      else if (y==0)
		{
		  Vector up = Vector(0, YWidth, z[x+(y+1)*nx] - z[x+y*nx]);
		  Vector down = Vector (0, -YWidth, z[x+(y-1)*nx] - z[x+y*nx]);
		  Vector right = Vector (XWidth, 0, z[x+1+y*nx] - z[x+y*nx]);
		  if ((x!=0) && (x!=nx-1))
		    normals[x] = (Cross(right, up)
				  + Cross(down, right)).Hat();
		}
	      else if ((x == nx-1) && (y != ny-1))
		{
		  Vector up = Vector(0, YWidth, z[x+(y+1)*nx] - z[x+y*nx]);
		  Vector down = Vector (0, -YWidth, z[x+(y-1)*nx] - z[x+y*nx]);
		  Vector left = Vector (-XWidth, 0, z[x-1+y*nx] - z[x+y*nx]);
		  if ((y != 0) && (y!=ny-1))
		    normals[x+y*nx] = (Cross(up, left) + Cross(left, down)).Hat();
		}
	      else if (y == ny-1)
		{
		  Vector down = Vector (0, -YWidth, z[x+(y-1)*nx] - z[x+y*nx]);
		  Vector left = Vector (-XWidth, 0, z[x-1+y*nx] - z[x+y*nx]);
		  Vector right = Vector (XWidth, 0, z[x+1+y*nx] - z[x+y*nx]);
		  if ((x!=0) && (x!=nx-1))
		    normals[x+y*nx] = (Cross(left, down) + Cross(down, right)).Hat();
		}
	      else
		{
		  Vector up = Vector(0, YWidth, z[x+(y+1)*nx] - z[x+y*nx]);
		  Vector down = Vector (0, -YWidth, z[x+(y-1)*nx] - z[x+y*nx]);
		  Vector left = Vector (-XWidth, 0, z[x-1+y*nx] - z[x+y*nx]);
		  Vector right = Vector (XWidth, 0, z[x+1+y*nx] - z[x+y*nx]);
		  normals[x + y*nx] = (Cross(up, left) 
				       + Cross(left, down)
				       + Cross(down, right) 
				       + Cross (right, up)).Hat();
		}
	    }
	}

    }

}

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

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 
{
  return true;
}

bool Heightfield::triangleIntersect(const Ray & ray, const Point & p1, 
				    const Point &p2, const Point &p3,
				    const Vector & n1,
				    const Vector & n2,
				    const Vector & n3,
				    Float * thitp, 
				    DifferentialGeometry *dg) const
{
  // stolen from the triangle class
  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;

  // Fill in [[DifferentialGeometry]] from triangle hit
  // Compute triangle partial derivatives

  // Interpolate $(u,v)$ triangle parametric coordinates
  Float b0 = 1. - b1 - b2;
  Float tu = b0*p1.x + b1*p2.x + b2*p3.x;
  Float tv = b0*p1.y + b1*p2.y + b2*p3.y;
  

  Vector dPdu1, dPdv1, dPdu2, dPdv2, dPdu3, dPdv3;

  CoordinateSystem(n1, &dPdu1, &dPdv1);
  CoordinateSystem(n2, &dPdu2, &dPdv2);
  CoordinateSystem(n3, &dPdu3, &dPdv3);

  Vector dPdu = b0*dPdu1 + b1*dPdu2 + b2*dPdu3;
  Vector dPdv = b0*dPdv1 + b1*dPdv2 + b2*dPdv3;

  if (Dot(ray.D, Cross(dPdu, dPdv)) < 0)
    {
      dPdu = E1;
      dPdv = E2;
    }

  *dg = DifferentialGeometry(ObjectToWorld(ray(t)), ObjectToWorld(dPdu), 
  			     ObjectToWorld(dPdv), tu, tv, this);
  *thitp = t;
  return true;

}

bool Heightfield::triangleIntersectP(const Ray & ray, const Point & p1, 
				    const Point &p2, const Point &p3) const
{
  // stolen from the triangle class
  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;

  return true;

}

bool Heightfield::Intersect (const Ray &r, Float *thitp, 
			     DifferentialGeometry *dg) const
{
  // Transform Ray to object space
  Ray ray = WorldToObject(r);

  // Check ray against overall grid bounds
  Float rayT = ray.maxt;
  if (bbox.Inside(ray(ray.mint)))
    rayT = ray.mint;
  else if (!bbox.IntersectP(ray, &rayT))
    return false;
  Point gridIntersect = ray(rayT);

  // Set up X stepping
  // Compute current x voxel
  int x = x2v(gridIntersect.x);
  if (x == XVoxels) 
    x--;
  else if (x < 0) 
    ++x;
  Assert(x >= 0 && x < XVoxels);
  Float NextXCrossing, DeltaX;
  int StepX, OutX;
  if (fabs(ray.D.x) < 1e-6) 
    {
      // Handle ray perpendicular to x
      NextXCrossing = INFINITY;
      DeltaX = 0;
      OutX = -1;
    }
  else if (ray.D.x > 0) 
    {
      // Handle ray with positive x direction
      NextXCrossing = rayT + (v2x(x+1) - gridIntersect.x)/ray.D.x;
      DeltaX = XWidth / ray.D.x;
      StepX = 1;
      OutX = XVoxels;
    }
  else 
    {
      // Handle ray with negative x direction
      NextXCrossing = rayT + (v2x(x) - gridIntersect.x)/ray.D.x;
      DeltaX = - XWidth / ray.D.x;
      StepX = -1;
      OutX = -1;
    }

  // Set up Y stepping
  int y = y2v( gridIntersect.y );
  if (y == YVoxels) 
    y--;
  if (y < 0) 
    ++y;
  Float NextYCrossing, DeltaY;
  int StepY, OutY;
	
  Assert( y >= 0 && y < YVoxels );
  if (fabs(ray.D.y) < 1e-6) 
    {
      NextYCrossing = INFINITY;
      DeltaY = 0;
      OutY = -1;
    }
  else if (ray.D.y < 0) 
    {
      NextYCrossing = rayT + ( v2y( y ) - gridIntersect.y )/ray.D.y;
      DeltaY = - YWidth / ray.D.y;
      StepY = OutY = -1;
    }
  else 
    {
      NextYCrossing = rayT + ( v2y( y+1 ) - gridIntersect.y )/ray.D.y;
      DeltaY = YWidth / ray.D.y;
      StepY = 1;
      OutY = YVoxels;
    }


  // Walk grid
  bool hitSomething = false;

  Float LastX, LastY;
  Point p1, p2, p3, p4;
  //while (!hitSomething)
  for(;;)
    {
      p1 = Point ((Float)x / (Float)XVoxels, 
		  (Float)y / (Float)YVoxels, 
		  z[y*nx + x]);
      p2 = Point ((Float)(x+1) / (Float)XVoxels, 
		  (Float)y / (Float)YVoxels, 
		  z[(x+1) + y*nx]);
      p3 = Point ((Float)x / (Float)XVoxels, 
		  (Float)(y+1) / (Float)YVoxels,  
		  z[x + (y+1)*nx]);
      p4 = Point ((Float)(x+1) / (Float)XVoxels, 
		  (Float)(y+1) / (Float)YVoxels, 
		  z[(x+1) + (y+1)*nx]);
      
      // my failed attempt at a min/max test
      /*
      Float minz = min(p1.z, min(p2.z, min(p3.z, p4.z)));
      Float maxz = max(p1.z, max(p2.z, max(p3.z, p4.z)));

      if (!(( (ray(NextXCrossing).z > maxz) && (ray(LastX).z > maxz))
	    || ((ray(NextXCrossing).z < minz) && (ray(LastX).z < minz))
	    || ((ray(NextYCrossing).z > maxz) && (ray(LastY).z > maxz))
	    || ((ray(NextYCrossing).z < minz) && (ray(LastY).z < minz))) )
      */
	{
	  
	  if (triangleIntersect (ray, p1, p2, p4, normals[x+y*nx], 
				 normals[x+1+y*nx], normals[x+1+(y+1)*nx], 
				 thitp, dg))
	    {
	      return true;
	    }
	  else if (triangleIntersect (ray, p4, p3, p1, normals[x+1+(y+1)*nx],
				      normals[x+(y+1)*nx], normals[x+y*nx],
				      thitp, dg))
	    {
	      return true;
	    }
	  //else hitSomething = false;
      
	}
      
      LastX = NextXCrossing;
      LastY = NextYCrossing;
      
      // increment
      if (NextXCrossing < NextYCrossing)
	{
	  // Step in X
	  NextXCrossing += DeltaX;
	  x += StepX;
	  if (x == OutX)
	    break;
	}
      else
	{
	  NextYCrossing += DeltaY;
	  y += StepY;
	  if (y == OutY)
	    break;
	}
    }

  return hitSomething;

}

bool Heightfield::IntersectP (const Ray &r) const
{
  // Transform Ray to object space
  Ray ray = WorldToObject(r);

  // Check ray against overall grid bounds
  Float rayT = ray.maxt;
  if (bbox.Inside(ray(ray.mint)))
    rayT = ray.mint;
  else if (!bbox.IntersectP(ray, &rayT))
    return false;
  Point gridIntersect = ray(rayT);
  
  // Set up X stepping
  // Compute current x voxel
  int x = x2v(gridIntersect.x);
  if (x == XVoxels) 
    x--;
  else if (x < 0) 
    ++x;
  Assert(x >= 0 && x < XVoxels);
  Float NextXCrossing, DeltaX;
  int StepX, OutX;
  if (fabs(ray.D.x) < 1e-6) 
    {
      // Handle ray perpendicular to x
      NextXCrossing = INFINITY;
      DeltaX = 0;
      OutX = -1;
    }
  else if (ray.D.x > 0) 
    {
      // Handle ray with positive x direction
      NextXCrossing = rayT + (v2x(x+1) - gridIntersect.x)/ray.D.x;
      DeltaX = XWidth / ray.D.x;
      StepX = 1;
      OutX = XVoxels;
    }
  else 
    {
      // Handle ray with negative x direction
      NextXCrossing = rayT + (v2x(x) - gridIntersect.x)/ray.D.x;
      DeltaX = - XWidth / ray.D.x;
      StepX = -1;
      OutX = -1;
    }

  // Set up Y stepping
  int y = y2v( gridIntersect.y );
  if (y == YVoxels) 
    y--;
  if (y < 0) 
    ++y;
  Float NextYCrossing, DeltaY;
  int StepY, OutY;
	
  Assert( y >= 0 && y < YVoxels );
  if (fabs(ray.D.y) < 1e-6) 
    {
      NextYCrossing = INFINITY;
      DeltaY = 0;
      OutY = -1;
    }
  else if (ray.D.y < 0) 
    {
      NextYCrossing = rayT + ( v2y( y ) - gridIntersect.y )/ray.D.y;
      DeltaY = - YWidth / ray.D.y;
      StepY = OutY = -1;
    }
  else 
    {
      NextYCrossing = rayT + ( v2y( y+1 ) - gridIntersect.y )/ray.D.y;
      DeltaY = YWidth / ray.D.y;
      StepY = 1;
      OutY = YVoxels;
    }


  // Walk grid
  bool hitSomething = false;

  Point p1, p2, p3, p4;
  //while (!hitSomething)
  for(;;)
    {
      //test triangles
      p1 = Point ((Float)x / (Float)XVoxels, 
			(Float)y / (Float)YVoxels, 
			z[y*nx + x]);
      p2 = Point ((Float)(x+1) / (Float)XVoxels, 
			(Float)y / (Float)YVoxels, 
			z[(x+1) + y*nx]);
      p3 = Point ((Float)x / (Float)XVoxels, 
			(Float)(y+1) / (Float)YVoxels,  
			z[x + (y+1)*nx]);
      p4 = Point ((Float)(x+1) / (Float)XVoxels, 
			(Float)(y+1) / (Float)YVoxels, 
			z[(x+1) + (y+1)*nx]);


      if (triangleIntersectP (ray, p1, p2, p4))
	{
	  return true;
	}
      else if (triangleIntersectP (ray, p4, p3, p1))
	{
	  return true;
	}
      // else hitSomething = false;
      
      
      // increment
      if (NextXCrossing < NextYCrossing)
	{
	  // Step in X
	  NextXCrossing += DeltaX;
	  x += StepX;
	  if (x == OutX)
	    break;
	}
      else
	{
	  NextYCrossing += DeltaY;
	  y += StepY;
	  if (y == OutY)
	    break;
	}
    }

  return hitSomething;

}

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);
}