#include "grid.h"
#include "argparser.h"

// Included files for OpenGL Rendering
#include <GL/gl.h>
#include <GL/glut.h>

// ============================================================================
// ============================================================================
// ============================================================================

void Grid::MarchingCubes(int x, int y, int z, int wireframe) const {
  assert (x >= 0 && x < nx);
  assert (y >= 0 && y < ny);
  assert (z >= 0 && z < nz);

  // split this cube into 8 cubes 
  // increases resolution and simplifies things
  for (int i = -1; i < 1; i++) {
    for (int j = -1; j < 1; j++) {
      for (int k = -1; k < 1; k++) {
	Vec3f verts[8];
	verts[0] = Vec3f(x+i*0.5    ,y+j*0.5    ,z+k*0.5    );
	verts[1] = Vec3f(x+i*0.5    ,y+j*0.5    ,z+k*0.5+0.5);
	verts[2] = Vec3f(x+i*0.5    ,y+j*0.5+0.5,z+k*0.5    );
	verts[3] = Vec3f(x+i*0.5    ,y+j*0.5+0.5,z+k*0.5+0.5);
	verts[4] = Vec3f(x+i*0.5+0.5,y+j*0.5    ,z+k*0.5    );
	verts[5] = Vec3f(x+i*0.5+0.5,y+j*0.5    ,z+k*0.5+0.5);
	verts[6] = Vec3f(x+i*0.5+0.5,y+j*0.5+0.5,z+k*0.5    );
	verts[7] = Vec3f(x+i*0.5+0.5,y+j*0.5+0.5,z+k*0.5+0.5);
	MarchingCubesHelper(verts,abs(i+j+k)%2,wireframe);
      }
    }
  }
}

// ============================================================================

void Grid::MarchingCubesHelper(Vec3f verts[8], int parity, int wireframe) const {

  // need to alternate orientation of central tetrahedron to ensure the diagonals line up
  if (parity) {
    MarchingTetras(verts[0],verts[5],verts[3],verts[6],wireframe);
    MarchingTetras(verts[0],verts[1],verts[3],verts[5],wireframe);
    MarchingTetras(verts[0],verts[4],verts[5],verts[6],wireframe);
    MarchingTetras(verts[0],verts[2],verts[6],verts[3],wireframe);
    MarchingTetras(verts[3],verts[7],verts[6],verts[5],wireframe);
  } else {
    MarchingTetras(verts[2],verts[4],verts[1],verts[7],wireframe);
    MarchingTetras(verts[5],verts[4],verts[7],verts[1],wireframe);
    MarchingTetras(verts[2],verts[3],verts[7],verts[1],wireframe);
    MarchingTetras(verts[4],verts[2],verts[1],verts[0],wireframe);
    MarchingTetras(verts[2],verts[7],verts[6],verts[4],wireframe);
  }
}

// ============================================================================

void Grid::MarchingTetras(Vec3f a, Vec3f b, Vec3f c, Vec3f d, int wireframe) const {

  float val_a = interpolateIsovalue(a);
  float val_b = interpolateIsovalue(b);
  float val_c = interpolateIsovalue(c);
  float val_d = interpolateIsovalue(d);

  // figure out how many of the vertices are on each side of the isosurface
  int count = 
    (val_a > args->isosurface) +
    (val_b > args->isosurface) +
    (val_c > args->isosurface) +
    (val_d > args->isosurface);

  // handle each case
  if (count == 4) { MarchingTetrasHelper4(a,b,c,d,wireframe); }
  else if (count == 3) {
    if (val_d <= args->isosurface) MarchingTetrasHelper3(a,b,c,d,wireframe);
    else if (val_c <= args->isosurface) MarchingTetrasHelper3(a,d,b,c,wireframe);
    else if (val_b <= args->isosurface) MarchingTetrasHelper3(a,c,d,b,wireframe);
    else if (val_a <= args->isosurface) MarchingTetrasHelper3(b,d,c,a,wireframe);
    else assert(0);
  } else if (count == 2) {
    if (val_a > args->isosurface && val_b > args->isosurface) MarchingTetrasHelper2(a,b,c,d,wireframe);
    else if (val_a > args->isosurface && val_c > args->isosurface) MarchingTetrasHelper2(a,c,d,b,wireframe);
    else if (val_a > args->isosurface && val_d > args->isosurface) MarchingTetrasHelper2(a,d,b,c,wireframe);
    else if (val_b > args->isosurface && val_c > args->isosurface) MarchingTetrasHelper2(b,c,a,d,wireframe);
    else if (val_b > args->isosurface && val_d > args->isosurface) MarchingTetrasHelper2(b,d,c,a,wireframe);
    else if (val_c > args->isosurface && val_d > args->isosurface) MarchingTetrasHelper2(c,d,a,b,wireframe);
    else assert(0);
  } else if (count == 1) {
    if (val_a > args->isosurface) MarchingTetrasHelper1(a,b,c,d,wireframe);
    else if (val_b > args->isosurface) MarchingTetrasHelper1(b,c,a,d,wireframe);
    else if (val_c > args->isosurface) MarchingTetrasHelper1(c,a,b,d,wireframe);
    else if (val_d > args->isosurface) MarchingTetrasHelper1(d,c,b,a,wireframe);
    else assert(0);
  } else {
    // count == 0, do nothing
  }
}

// ============================================================================

// when all 4 vertices are within the volume
void Grid::MarchingTetrasHelper4(Vec3f a, Vec3f b, Vec3f c, Vec3f d, int wireframe) const {
  float val_a = interpolateIsovalue(a);
  float val_b = interpolateIsovalue(b);
  float val_c = interpolateIsovalue(c);
  float val_d = interpolateIsovalue(d);
  assert (val_a > args->isosurface && val_b > args->isosurface && val_c > args->isosurface && val_d > args->isosurface);

  drawIfBoundary(a,b,c,wireframe);
  drawIfBoundary(a,d,b,wireframe);
  drawIfBoundary(b,d,c,wireframe);
  drawIfBoundary(c,d,a,wireframe);
}

// when 3 vertices are within the volume
void Grid::MarchingTetrasHelper3(Vec3f a, Vec3f b, Vec3f c, Vec3f d, int wireframe) const {
  float val_a = interpolateIsovalue(a);
  float val_b = interpolateIsovalue(b);
  float val_c = interpolateIsovalue(c);
  float val_d = interpolateIsovalue(d);
  assert (val_a > args->isosurface && val_b > args->isosurface && val_c > args->isosurface && val_d <= args->isosurface);

  float ad_frac = (args->isosurface - val_d) / (val_a - val_d);
  assert (ad_frac >= 0 && ad_frac <= 1);
  float bd_frac = (args->isosurface - val_d) / (val_b - val_d);
  assert (bd_frac >= 0 && bd_frac <= 1);
  float cd_frac = (args->isosurface - val_d) / (val_c - val_d);
  assert (cd_frac >= 0 && cd_frac <= 1);
  Vec3f ad = ad_frac*a + (1-ad_frac)*d;
  Vec3f bd = bd_frac*b + (1-bd_frac)*d;
  Vec3f cd = cd_frac*c + (1-cd_frac)*d;

  drawTriangle(ad,cd,bd,wireframe);
  drawIfBoundary(a,ad,bd,wireframe);
  drawIfBoundary(a,bd,b,wireframe);
  drawIfBoundary(b,bd,cd,wireframe);
  drawIfBoundary(b,cd,c,wireframe);
  drawIfBoundary(c,cd,ad,wireframe);
  drawIfBoundary(c,ad,a,wireframe);
  drawIfBoundary(a,b,c,wireframe);
}

// when 2 vertices are within the volume
void Grid::MarchingTetrasHelper2(Vec3f a, Vec3f b, Vec3f c, Vec3f d, int wireframe) const {
  float val_a = interpolateIsovalue(a);
  float val_b = interpolateIsovalue(b);
  float val_c = interpolateIsovalue(c);
  float val_d = interpolateIsovalue(d);
  assert (val_a > args->isosurface && val_b > args->isosurface && val_c <= args->isosurface && val_d <= args->isosurface);

  float ac_frac = (args->isosurface - val_c) / (val_a - val_c);
  assert (ac_frac >= 0 && ac_frac <= 1);
  float ad_frac = (args->isosurface - val_d) / (val_a - val_d);
  assert (ad_frac >= 0 && ad_frac <= 1);
  float bc_frac = (args->isosurface - val_c) / (val_b - val_c);
  assert (bc_frac >= 0 && bc_frac <= 1);
  float bd_frac = (args->isosurface - val_d) / (val_b - val_d);
  assert (bd_frac >= 0 && bd_frac <= 1);
  Vec3f ac = ac_frac*a + (1-ac_frac)*c;
  Vec3f ad = ad_frac*a + (1-ad_frac)*d;
  Vec3f bc = bc_frac*b + (1-bc_frac)*c;
  Vec3f bd = bd_frac*b + (1-bd_frac)*d;

  drawTriangle(ac,bc,ad,wireframe);
  drawTriangle(bc,bd,ad,wireframe);
  drawIfBoundary(a,ac,ad,wireframe);
  drawIfBoundary(b,bd,bc,wireframe);
  drawIfBoundary(a,ad,bd,wireframe);
  drawIfBoundary(a,bd,b,wireframe);
  drawIfBoundary(a,bc,ac,wireframe);
  drawIfBoundary(a,b,bc,wireframe);
}

// when 1 vertex is within the volume
void Grid::MarchingTetrasHelper1(Vec3f a, Vec3f b, Vec3f c, Vec3f d, int wireframe) const {
  float val_a = interpolateIsovalue(a);
  float val_b = interpolateIsovalue(b);
  float val_c = interpolateIsovalue(c);
  float val_d = interpolateIsovalue(d);
  assert (val_a > args->isosurface && val_b <= args->isosurface && val_c <= args->isosurface && val_d <= args->isosurface);

  float ab_frac = (args->isosurface - val_b) / (val_a - val_b);
  assert (ab_frac >= 0 && ab_frac <= 1);
  float ac_frac = (args->isosurface - val_c) / (val_a - val_c);
  assert (ac_frac >= 0 && ac_frac <= 1);
  float ad_frac = (args->isosurface - val_d) / (val_a - val_d);
  assert (ad_frac >= 0 && ad_frac <= 1);
  Vec3f ab = ab_frac*a + (1-ab_frac)*b;
  Vec3f ac = ac_frac*a + (1-ac_frac)*c;
  Vec3f ad = ad_frac*a + (1-ad_frac)*d;

  drawTriangle(ab,ad,ac,wireframe);
  drawIfBoundary(a,ab,ac,wireframe);
  drawIfBoundary(a,ac,ad,wireframe);
  drawIfBoundary(a,ad,ab,wireframe);
}

// ============================================================================
// ============================================================================
// ============================================================================

float Grid::triInterpolate(float x_frac, float y_frac, float z_frac,
			   float a, float b, float c, float d, float e, float f, float g, float h) const {
  assert (x_frac >= 0 && x_frac <= 1);
  assert (y_frac >= 0 && y_frac <= 1);
  assert (z_frac >= 0 && z_frac <= 1);

  // trilinear interpolation
  float ab = (1-z_frac)*a + z_frac*b;
  float cd = (1-z_frac)*c + z_frac*d;
  float ef = (1-z_frac)*e + z_frac*f;
  float gh = (1-z_frac)*g + z_frac*h;
  float abcd = (1-y_frac)*ab + y_frac*cd;
  float efgh = (1-y_frac)*ef + y_frac*gh;
  float abcdefgh = (1-x_frac)*abcd + x_frac*efgh;

  return abcdefgh;
}

float Grid::interpolateIsovalue(Vec3f v) const {

  float x = v.x();
  float y = v.y();
  float z = v.z();

  // get the values at the corners
  float a = getIsovalue(int(floor(x)),int(floor(y)),int(floor(z)));
  float b = getIsovalue(int(floor(x)),int(floor(y)),int( ceil(z)));
  float c = getIsovalue(int(floor(x)),int( ceil(y)),int(floor(z)));
  float d = getIsovalue(int(floor(x)),int( ceil(y)),int( ceil(z)));
  float e = getIsovalue(int( ceil(x)),int(floor(y)),int(floor(z)));
  float f = getIsovalue(int( ceil(x)),int(floor(y)),int( ceil(z)));
  float g = getIsovalue(int( ceil(x)),int( ceil(y)),int(floor(z)));
  float h = getIsovalue(int( ceil(x)),int( ceil(y)),int( ceil(z)));

  float x_frac = x - (floor(x));
  float y_frac = y - (floor(y));
  float z_frac = z - (floor(z));

  return triInterpolate(x_frac,y_frac,z_frac,a,b,c,d,e,f,g,h);
}

// ============================================================================

void Grid::insertColor(float value) const {
  // 1   -> red
  // 0.8 -> orange
  // 0.6 -> yellow
  // 0.4 -> green
  // 0.2 -> blue
  // 0   -> white
  if (value < 0) value = 0;
  if (value > 1) value = 1;
  if (value > 0.6) {      
    glColor3f(1.0,1-(value-0.6)/0.4,0); return; }
  if (value > 0.4) {
    glColor3f((value-0.4)/0.2,1.0,0); return; }
  if (value > 0.2) {
    glColor3f((value-0.4)/0.2,1.0,0); return; }
  if (value > 0.2) {
    glColor3f(0,(value-0.2)/0.2,1-(value-0.2)/0.2); return; }
  else {
    glColor3f((value-0.2)/0.2,(value-0.2)/0.2,1.0); return; }
}

void Grid::insertVertex(Vec3f v) const { 
  float value = interpolateIsovalue(v);
  insertColor(value);
  glVertex3f(v.x(),v.y(),v.z()); 
}

void Grid::insertNormal(const Vec3f &p1, const Vec3f &p2, const Vec3f &p3) const {
  Vec3f v12 = p2;
  v12 -= p1;
  Vec3f v23 = p3;
  v23 -= p2;
  Vec3f normal;
  Vec3f::Cross3(normal,v12,v23);
  normal.Normalize();
  glNormal3f(normal.x(), normal.y(), normal.z());
}

void Grid::drawTriangle(const Vec3f &p1, const Vec3f &p2, const Vec3f &p3, int wireframe) const {
  insertNormal(p1,p2,p3);
  insertVertex(p1);
  insertVertex(p2);
  insertVertex(p3);
  if (wireframe) {
    glEnd();
    glColor3f(0,0,0);
    glDisable(GL_LIGHTING);
    glBegin(GL_LINES);
    glVertex3f(p1.x(),p1.y(),p1.z()); 
    glVertex3f(p2.x(),p2.y(),p2.z()); 
    glVertex3f(p2.x(),p2.y(),p2.z()); 
    glVertex3f(p3.x(),p3.y(),p3.z()); 
    glVertex3f(p3.x(),p3.y(),p3.z()); 
    glVertex3f(p1.x(),p1.y(),p1.z()); 
    glEnd();
    glEnable(GL_LIGHTING);
    glBegin(GL_TRIANGLES);
  }
}

void Grid::drawIfBoundary(const Vec3f &p1, const Vec3f &p2, const Vec3f &p3, int wireframe) const {
  if ((p1.x() <= -0.499 && p2.x() <= -0.499 && p3.x() <= -0.499) ||
      (p1.y() <= -0.499 && p2.y() <= -0.499 && p3.y() <= -0.499) ||
      (p1.z() <= -0.499 && p2.z() <= -0.499 && p3.z() <= -0.499) ||
      (p1.x() >= nx-0.501 && p2.x() >= nx-0.501 && p3.x() >= nx-0.501) ||
      (p1.y() >= ny-0.501 && p2.y() >= ny-0.501 && p3.y() >= ny-0.501) ||
      (p1.z() >= nz-0.501 && p2.z() >= nz-0.501 && p3.z() >= nz-0.501)) {
    drawTriangle(p1,p2,p3,wireframe);
  }
}

// ============================================================================