#include <float.h>
#include <assert.h>
#include "meshEdit.h"
#include "mutablePriorityQueue.h"
#include "error_dialog.h"
#include <iostream>

using namespace std;

namespace CS248 {

VertexIter HalfedgeMesh::splitEdge(EdgeIter e0) {
  // TODO: (meshEdit)
  // This method should split the given edge and return an iterator to the
  // newly inserted vertex. The halfedge of this vertex should point along
  // the edge that was split, rather than the new edges.
	// Halfedges
	if(!e0->isBoundary()) {
		Vector3D oldMidpoint = e0->centroid();
		HalfedgeIter h0 = e0->halfedge();
		HalfedgeIter h1 = h0->next();
		HalfedgeIter h2 = h1->next();
		HalfedgeIter h3 = h0->twin();
		HalfedgeIter h4 = h3->next();
		HalfedgeIter h5 = h4->next();
		HalfedgeIter h6 = h1->twin();
		HalfedgeIter h7 = h2->twin();
		HalfedgeIter h8 = h4->twin();
		HalfedgeIter h9 = h5->twin();

		// Vertices
		VertexIter v0 = h0->vertex();
		VertexIter v1 = h3->vertex();
		VertexIter v2 = h2->vertex();
		VertexIter v3 = h5->vertex();

		// Edges
		EdgeIter e1 = h1->edge();
		EdgeIter e2 = h2->edge();
		EdgeIter e3 = h4->edge();
		EdgeIter e4 = h5->edge();

		// Faces
		FaceIter f0 = h0->face();
		FaceIter f1 = h3->face();

		// New Vertice
		VertexIter v4 = this->newVertex();

		// New Halfedges
		HalfedgeIter h10 = this->newHalfedge();
		HalfedgeIter h11 = this->newHalfedge();
		HalfedgeIter h12 = this->newHalfedge();
		HalfedgeIter h13 = this->newHalfedge();
		HalfedgeIter h14 = this->newHalfedge();
		HalfedgeIter h15 = this->newHalfedge();

		// New Faces
		FaceIter f2 = this->newFace();
		FaceIter f3 = this->newFace();

		// New Edges
		EdgeIter e5 = this->newEdge();
		EdgeIter e6 = this->newEdge();
		EdgeIter e7 = this->newEdge();

		// UPDATES
		this->deleteEdge(e0); // remove edge
		e0 = this->newEdge(); // create new edge

		// Halfedges
		h0->next() = h1;
		h0->twin() = h3;
		h0->vertex() = v4; // new
		h0->edge() = e0;
		h0->face() = f0;

		h1->next() = h11; // new
		h1->twin() = h6;
		h1->vertex() = v1;
		h1->edge() = e1;
		h1->face() = f0;

		h2->next() = h13; // new
		h2->twin() = h7;
		h2->vertex() = v2;
		h2->edge() = e2;
		h2->face() = f2; // new

		h3->next() = h10; // new
		h3->twin() = h0;
		h3->vertex() = v1;
		h3->edge() = e0;
		h3->face() = f1;

		h4->next() = h15; // new
		h4->twin() = h8;
		h4->vertex() = v0;
		h4->edge() = e3;
		h4->face() = f3; // new

		h5->next() = h3;
		h5->twin() = h9;
		h5->vertex() = v3;
		h5->edge() = e4;
		h5->face() = f1;

		h6->next() = h6->next();
		h6->twin() = h1;
		h6->vertex() = v2;
		h6->edge() = e1;
		h6->face() = h6->face();

		h7->next() = h7->next();
		h7->twin() = h2;
		h7->vertex() = v0;
		h7->edge() = e2;
		h7->face() = h7->face();

		h8->next() = h8->next();
		h8->twin() = h4;
		h8->vertex() = v3;
		h8->edge() = e3;
		h8->face() = h8->face();

		h9->next() = h9->next();
		h9->twin() = h5;
		h9->vertex() = v1;
		h9->edge() = e4;
		h9->face() = h9->face();

		// NEW
		h10->next() = h5;
		h10->twin() = h15;
		h10->vertex() = v4;
		h10->edge() = e6;
		h10->face() = f1;

		h11->next() = h0;
		h11->twin() = h12;
		h11->vertex() = v2;
		h11->edge() = e7;
		h11->face() = f0;

		h12->next() = h2;
		h12->twin() = h11;
		h12->vertex() = v4;
		h12->edge() = e7;
		h12->face() = f2;

		h13->next() = h12;
		h13->twin() = h14;
		h13->vertex() = v0;
		h13->edge() = e5;
		h13->face() = f2;

		h14->next() = h4;
		h14->twin() = h13;
		h14->vertex() = v4;
		h14->edge() = e5;
		h14->face() = f3;

		h15->next() = h14;
		h15->twin() = h10;
		h15->vertex() = v3;
		h15->edge() = e6;
		h15->face() = f3;

		// Vertices
		v0->halfedge() = h13;
		v1->halfedge() = h3;
		v2->halfedge() = h11;
		v3->halfedge() = h15;
		v4->halfedge() = h0; // IMPORTANT!!

		// Edges
		e0->halfedge() = h0;
		e1->halfedge() = h1;
		e2->halfedge() = h2;
		e3->halfedge() = h4;
		e4->halfedge() = h5;
		e5->halfedge() = h14;
		e6->halfedge() = h10;
		e7->halfedge() = h12;

		// Faces
		f0->halfedge() = h0;
		f1->halfedge() = h3;
		f2->halfedge() = h13;
		f3->halfedge() = h14;

		v4->position = oldMidpoint; // new position
		return v4;
	} else {
		Vector3D oldMidpoint = e0->centroid();
		HalfedgeIter h0 = e0->halfedge();
		if(h0->isBoundary()) {
			h0 = h0->twin();
		}
		HalfedgeIter h1 = h0->next();
		HalfedgeIter h2 = h1->next();
		HalfedgeIter h3 = h0->twin();
		HalfedgeIter h4 = h1->twin();
		HalfedgeIter h5 = h2->twin();

		// Vertices
		VertexIter v0 = h0->vertex();
		VertexIter v1 = h1->vertex();
		VertexIter v2 = h2->vertex();

		// Edges
		EdgeIter e1 = h1->edge();
		EdgeIter e2 = h2->edge();

		// Faces
		FaceIter f0 = h0->face();

		// New Vertice
		VertexIter v3 = this->newVertex();

		// New Halfedges
		HalfedgeIter h6 = this->newHalfedge();
		HalfedgeIter h7 = this->newHalfedge();
		HalfedgeIter h8 = this->newHalfedge();
		HalfedgeIter h9 = this->newHalfedge();

		// New Faces
		FaceIter f1 = this->newFace();

		// New Edges
		EdgeIter e3 = this->newEdge();
		EdgeIter e4 = this->newEdge();

		// UPDATES
		this->deleteEdge(e0); // remove edge
		e0 = this->newEdge(); // create new edge

		h0->next() = h1;
		h0->twin() = h3;
		h0->vertex() = v3; // new
		h0->edge() = e0;
		h0->face() = f0;

		h1->next() = h6; // new
		h1->twin() = h4;
		h1->vertex() = v1;
		h1->edge() = e1;
		h1->face() = f0;

		h2->next() = h8; // new
		h2->twin() = h5;
		h2->vertex() = v2;
		h2->edge() = e2;
		h2->face() = f1; // new

		HalfedgeIter h3OldNext = h3->next();
		h3->next() = h9; // new
		h3->twin() = h0;
		h3->vertex() = v1;
		h3->edge() = e0;
		h3->face() = h3->face();

		h4->next() = h4->next(); // new
		h4->twin() = h1;
		h4->vertex() = v2;
		h4->edge() = e1;
		h4->face() = h4->face(); // new

		h5->next() = h5->next();
		h5->twin() = h2;
		h5->vertex() = v0;
		h5->edge() = e2;
		h5->face() = h5->face();

		h6->next() = h0;
		h6->twin() = h7;
		h6->vertex() = v2;
		h6->edge() = e4;
		h6->face() = f0;

		h7->next() = h2;
		h7->twin() = h6;
		h7->vertex() = v3;
		h7->edge() = e4;
		h7->face() = f1;

		h8->next() = h7;
		h8->twin() = h9;
		h8->vertex() = v0;
		h8->edge() = e3;
		h8->face() = f1;

		h9->next() = h3OldNext;
		h9->twin() = h8;
		h9->vertex() = v3;
		h9->edge() = e3;
		h9->face() = h3->face();

		v0->halfedge() = h8;
		v1->halfedge() = h1;
		v2->halfedge() = h2;
		v3->halfedge() = h0;

		e0->halfedge() = h0;
		e1->halfedge() = h1;
		e2->halfedge() = h2;
		e3->halfedge() = h8;
		e4->halfedge() = h6;

		f0->halfedge() = h0;
		f1->halfedge() = h8;

		v3->position = oldMidpoint;
		return v3;
	}

}

VertexIter HalfedgeMesh::collapseEdge(EdgeIter e) {
	// *** Extra Credit ***
	// TODO: (meshEdit)
	// This method should collapse the given edge and return an iterator to
	// the new vertex created by the collapse.
	HalfedgeIter h = e->halfedge();
	HalfedgeIter twin = h->twin();
	FaceIter f0 = h->face();
	FaceIter f1 = twin->face();
	VertexIter v0 = h->vertex();
	VertexIter v1 = twin->vertex();
	// use degree to detect if face with edge e is a triangle or not
	// only two faces; one on each side
	// BELOW WORKS IF NEITHER IS A TRIANGLE
	HalfedgeIter h0 = h->next(); // placeholder value... change accordingly
	HalfedgeIter h1 = h0;
	HalfedgeIter h2 = twin->next();
	HalfedgeIter h3 = h2;
	cout << f0->degree() << endl;
	cout << f1->degree() << endl;
	if (f0->degree() == 3) {
		this->eraseEdge(h0->edge());
		h0 = h->next();
		h1 = h0;
		cout << "erase 1" << endl;
	}
	if (f1->degree() == 3) {
		this->eraseEdge(h2->edge());
		h2 = twin->next();
		h3 = h2;
		cout << "erase 2" << endl;
	}

	cout << "erase edge worked!" << endl;

	while (h1->next() != h) {
		h1 = h1->next();
	}
	h1->next() = h0;
	f0->halfedge() = h0;
	v1->halfedge() = h0;
	this->deleteHalfedge(h);
	cout << "erased one halfedge!" << endl;

	while (h3->next() != twin) {
		h3 = h3->next();
	}
	h3->next() = h2;
	f1->halfedge() = h2;
	v0->halfedge() = h2;
	this->deleteHalfedge(twin);

	cout << "erased the other!" << endl;

	this->deleteEdge(e);
	// now merge two vertices together by removing v0
	HalfedgeIter step = h1;
	HalfedgeIter stop;
	while (step != h3) {
		step = step->twin();
		stop = step;
		step->vertex() = v1; // set vertex for each halfedge going out of v0 to be v1 instead
		while (step->next() != stop) {
			step = step->next();
		}
	}

	cout << "DONE" << endl;
	this->deleteVertex(v0);
	return v1;
}

VertexIter HalfedgeMesh::collapseFace(FaceIter f) {
	// *** Extra Credit ***
	// TODO: (meshEdit)
	// This method should collapse the given face and return an iterator to
	// the new vertex created by the collapse.
	HalfedgeIter h = f->halfedge();
	EdgeIter e = h->edge();
	HalfedgeIter hnext = h;
	VertexIter v;
	Size total = f->degree();
	cout << total << endl;
	do {
		e = hnext->edge();
		hnext = hnext->next();
		f->halfedge() = hnext;
		if (f->degree() == 3) {
			cout << "3!!!" << endl;
			HalfedgeIter h = e->halfedge();
			HalfedgeIter twin = h->twin();
			FaceIter f0 = h->face();
			FaceIter f1 = twin->face();
			VertexIter v0 = h->vertex();
			VertexIter v1 = twin->vertex();
			// use degree to detect if face with edge e is a triangle or not
			// only two faces; one on each side
			// BELOW WORKS IF NEITHER IS A TRIANGLE
			HalfedgeIter h0 = h->next(); // placeholder value... change accordingly
			HalfedgeIter h1 = h0;
			HalfedgeIter h2 = twin->next();
			HalfedgeIter h3 = h2;
			while (h1->next() != h) {
				h1 = h1->next();
			}
			h1->next() = h0;
			f0->halfedge() = h0;
			v1->halfedge() = h0;
			this->deleteHalfedge(h);

			while (h3->next() != twin) {
				h3 = h3->next();
			}
			h3->next() = h2;
			f1->halfedge() = h2;
			v0->halfedge() = h2;
			this->deleteHalfedge(twin);

			this->deleteEdge(e);
			// now merge two vertices together by removing v0
			HalfedgeIter step = h1;
			HalfedgeIter stop;
			while (step != h3) {
				step = step->twin();
				stop = step;
				step->vertex() = v1; // set vertex for each halfedge going out of v0 to be v1 instead
				while (step->next() != stop) {
					step = step->next();
				}
			}
			this->deleteVertex(v0);
		}
		else {
			cout << "edge collapse" << endl;
			v = this->collapseEdge(e);
		}
		total--;
	} while (total > 1);
	cout << "done!" << endl;
	return v;
}

FaceIter HalfedgeMesh::eraseVertex(VertexIter v) {
  HalfedgeIter h = v->halfedge(); // starting halfedge
  FaceIter f; // face to be kept
  // iterate over all the halfedges of faces that go around the vertice to be deleted
  HalfedgeIter hnext;
  EdgeIter e;
  int degree = v->degree();
  for(int i = 0; i < degree; i++) {
  	hnext = h->twin()->next();
  	e = h->edge();
  	f = this->eraseEdge(e);
  	h = hnext;
  }
  this->deleteVertex(v);

  return f;
}

FaceIter HalfedgeMesh::eraseEdge(EdgeIter e) {
  // *** Extra Credit ***
  HalfedgeIter h0 = e->halfedge();
  h0->name = "h0";
  HalfedgeIter h1 = h0->twin();
  h1->name = "h1";
  VertexIter v0 = h0->vertex();
  VertexIter v1 = h1->vertex();
  HalfedgeIter v0newNext = h1->next();
  v0newNext->name = "v0nn";
  HalfedgeIter v1newNext = h0->next();
  v1newNext->name = "v1nn";
  v0->halfedge() = v0newNext;
  v1->halfedge() = v1newNext;

  HalfedgeIter current = v0newNext->twin();
  while(current->next() != h0) {
  	current = current->next()->twin();
  }
  current->next() = v0newNext;
  current->name = "v0nc";


  current = v1newNext->twin();
  while(current->next() != h1) {
  	current = current->next()->twin();
  }
  current->next() = v1newNext;
  current->name = "v1nc";

  FaceIter mergedFace = v0newNext->face();
  FaceIter faceToErase = h0->face();
  current = v0newNext;
  if(v0newNext == h0) {
  	cout << "Making sure..." << endl;
  	mergedFace = v1newNext->face();
  	faceToErase = h1->face();
  	current = v1newNext;
  }
  mergedFace->halfedge() = current;
  //cout << current->name << endl;
  current = current->next();
  while(current != mergedFace->halfedge()) {
    current->face() = mergedFace;
    //cout << current->name << endl;
    current = current->next();
  }

  this->deleteHalfedge(h0);
  this->deleteHalfedge(h1);
  if(faceToErase != mergedFace) {
  	this->deleteFace(faceToErase);
  }

  this->deleteEdge(e);
  return mergedFace;
}

EdgeIter HalfedgeMesh::flipEdge(EdgeIter e0) {
	if (e0->isBoundary())
		return e0;
	// don't flip if tetrahedron...
	// need to make it able to do n-gon
	// more edge cases?
	// make sure number of faces stays same

	// PHASE I
	// Halfedges
	HalfedgeIter h0 = e0->halfedge();
	HalfedgeIter h1 = h0->next();
	HalfedgeIter h2 = h1->next();
	HalfedgeIter h3 = h0->twin();
	HalfedgeIter h4 = h3->next();
	HalfedgeIter h5 = h4->next();
	HalfedgeIter h6 = h1->twin();
	HalfedgeIter h7 = h2->twin();
	HalfedgeIter h8 = h4->twin();
	HalfedgeIter h9 = h5->twin();

	// Vertices
	VertexIter v0 = h0->vertex();
	VertexIter v1 = h3->vertex();
	VertexIter v2 = h2->vertex();
	VertexIter v3 = h5->vertex();

	// Edges
	EdgeIter e1 = h1->edge();
	EdgeIter e2 = h2->edge();
	EdgeIter e3 = h4->edge();
	EdgeIter e4 = h5->edge();

	// Faces
	FaceIter f0 = h0->face();
	FaceIter f1 = h3->face();

	// PHASE III
	// Halfedges
	h0->next() = h1;
	h0->twin() = h3;
	h0->vertex() = v3;
	h0->edge() = e0;
	h0->face() = f0;

	h1->next() = h2;
	h1->twin() = h7;
	h1->vertex() = v2;
	h1->edge() = e2;
	h1->face() = f0;

	h2->next() = h0;
	h2->twin() = h8;
	h2->vertex() = v0;
	h2->edge() = e3;
	h2->face() = f0;

	h3->next() = h4;
	h3->twin() = h0;
	h3->vertex() = v2;
	h3->edge() = e0;
	h3->face() = f1;

	h4->next() = h5;
	h4->twin() = h9;
	h4->vertex() = v3;
	h4->edge() = e4;
	h4->face() = f1;

	h5->next() = h3;
	h5->twin() = h6;
	h5->vertex() = v1;
	h5->edge() = e1;
	h5->face() = f1;

	h6->next() = h6->next();
	h6->twin() = h5;
	h6->vertex() = v2;
	h6->edge() = e1;
	h6->face() = h6->face();

	h7->next() = h7->next();
	h7->twin() = h1;
	h7->vertex() = v0;
	h7->edge() = e2;
	h7->face() = h7->face();

	h8->next() = h8->next();
	h8->twin() = h2;
	h8->vertex() = v3;
	h8->edge() = e3;
	h8->face() = h8->face();

	h9->next() = h9->next();
	h9->twin() = h4;
	h9->vertex() = v1;
	h9->edge() = e4;
	h9->face() = h9->face();

	// Vertices
	v0->halfedge() = h2;
	v1->halfedge() = h5;
	v2->halfedge() = h3;
	v3->halfedge() = h0;

	// Edges
	e0->halfedge() = h0;
	e1->halfedge() = h5;
	e2->halfedge() = h1;
	e3->halfedge() = h2;
	e4->halfedge() = h4;

	// Faces
	f0->halfedge() = h3;
	f1->halfedge() = h0;
	return e0;
}

void HalfedgeMesh::subdivideQuad(bool useCatmullClark) {
  // Unlike the local mesh operations (like bevel or edge flip), we will perform
  // subdivision by splitting *all* faces into quads "simultaneously."  Rather
  // than operating directly on the halfedge data structure (which as you've
  // seen
  // is quite difficult to maintain!) we are going to do something a bit nicer:
  //
  //    1. Create a raw list of vertex positions and faces (rather than a full-
  //       blown halfedge mesh).
  //
  //    2. Build a new halfedge mesh from these lists, replacing the old one.
  //
  // Sometimes rebuilding a data structure from scratch is simpler (and even
  // more
  // efficient) than incrementally modifying the existing one.  These steps are
  // detailed below.

  // TODO Step I: Compute the vertex positions for the subdivided mesh.  Here
  // we're
  // going to do something a little bit strange: since we will have one vertex
  // in
  // the subdivided mesh for each vertex, edge, and face in the original mesh,
  // we
  // can nicely store the new vertex *positions* as attributes on vertices,
  // edges,
  // and faces of the original mesh.  These positions can then be conveniently
  // copied into the new, subdivided mesh.
  // [See subroutines for actual "TODO"s]
  if (useCatmullClark) {
    computeCatmullClarkPositions();
  } else {
    computeLinearSubdivisionPositions();
  }

  //cout << "divisions worked!" << endl;
  // TODO Step II: Assign a unique index (starting at 0) to each vertex, edge,
  // and
  // face in the original mesh.  These indices will be the indices of the
  // vertices
  // in the new (subdivided mesh).  They do not have to be assigned in any
  // particular
  // order, so long as no index is shared by more than one mesh element, and the
  // total number of indices is equal to V+E+F, i.e., the total number of
  // vertices
  // plus edges plus faces in the original mesh.  Basically we just need a
  // one-to-one
  // mapping between original mesh elements and subdivided mesh vertices.
  // [See subroutine for actual "TODO"s]
  assignSubdivisionIndices();
  //cout << "indices worked!" << endl;

  // TODO Step III: Build a list of quads in the new (subdivided) mesh, as
  // tuples of
  // the element indices defined above.  In other words, each new quad should be
  // of
  // the form (i,j,k,l), where i,j,k and l are four of the indices stored on our
  // original mesh elements.  Note that it is essential to get the orientation
  // right
  // here: (i,j,k,l) is not the same as (l,k,j,i).  Indices of new faces should
  // circulate in the same direction as old faces (think about the right-hand
  // rule).
  // [See subroutines for actual "TODO"s]
  vector<vector<Index> > subDFaces;
  vector<Vector3D> subDVertices;
  buildSubdivisionFaceList(subDFaces);
  //cout << "faces worked!" << endl;
  buildSubdivisionVertexList(subDVertices);
  //cout << "vertices worked!" << endl;
  // TODO Step IV: Pass the list of vertices and quads to a routine that clears
  // the
  // internal data for this halfedge mesh, and builds new halfedge data from
  // scratch,
  // using the two lists.
  rebuild(subDFaces, subDVertices);
  //cout << "rebuild worked! All done!" << endl;
}

/**
 * Compute new vertex positions for a mesh that splits each polygon
 * into quads (by inserting a vertex at the face midpoint and each
 * of the edge midpoints).  The new vertex positions will be stored
 * in the members Vertex::newPosition, Edge::newPosition, and
 * Face::newPosition.  The values of the positions are based on
 * simple linear interpolation, e.g., the edge midpoints and face
 * centroids.
 */
void HalfedgeMesh::computeLinearSubdivisionPositions() {
  // TODO For each vertex, assign Vertex::newPosition to
  // its original position, Vertex::position.
	for (VertexIter v = this->verticesBegin(); v != this->verticesEnd(); v++) {
		v->newPosition = v->position;
	}
  // TODO For each edge, assign the midpoint of the two original
  // positions to Edge::newPosition.
	for (EdgeIter e = this->edgesBegin(); e != this->edgesEnd(); e++) {
		e->newPosition = e->centroid();
	}
  // TODO For each face, assign the centroid (i.e., arithmetic mean)
  // of the original vertex positions to Face::newPosition.  Note
  // that in general, NOT all faces will be triangles!
	for (FaceIter f = this->facesBegin(); f != this->facesEnd(); f++) {
		f->newPosition = f->centroid();
	}

  //showError("computeLinearSubdivisionPositions() not implemented.");
}

/**
 * Compute new vertex positions for a mesh that splits each polygon
 * into quads (by inserting a vertex at the face midpoint and each
 * of the edge midpoints).  The new vertex positions will be stored
 * in the members Vertex::newPosition, Edge::newPosition, and
 * Face::newPosition.  The values of the positions are based on
 * the Catmull-Clark rules for subdivision.
 */
void HalfedgeMesh::computeCatmullClarkPositions() {
  // TODO The implementation for this routine should be
  // a lot like HalfedgeMesh::computeLinearSubdivisionPositions(),
  // except that the calculation of the positions themsevles is
  // slightly more involved, using the Catmull-Clark subdivision
  // rules. (These rules are outlined in the Developer Manual.)

  // TODO face
	for (FaceIter f = this->facesBegin(); f != this->facesEnd(); f++) {
		f->newPosition = f->centroid();
	}

  // TODO edges
	for (EdgeIter e = this->edgesBegin(); e != this->edgesEnd(); e++) {
		if(e->isBoundary()) {
			e->newPosition = e->centroid();
		} else {
			e->newPosition = ((*e).halfedge()->face()->newPosition + (*e).halfedge()->twin()->face()->newPosition + (*e).halfedge()->vertex()->position + (*e).halfedge()->twin()->vertex()->position) / 4;
		}

	}

  // TODO vertices
	Vector3D q, r;
	Size n;
	HalfedgeIter h;
	for (VertexIter v = this->verticesBegin(); v != this->verticesEnd(); v++) {

		if(v->isBoundary()) {
			HalfedgeIter h = v->halfedge();
			EdgeIter e1, e2;
			int foundEdges = 0;
			do {
              if(h->edge()->isBoundary()) {
              	if(foundEdges == 0) {
              		e1 = h->edge();
              	} else {
              		e2 = h->edge();
              	}
              	foundEdges++;
              }
              h = h->twin()->next();
			}while(foundEdges < 2);
			v->newPosition = (3.0 / 4.0) * v->position + (1.0 / 8.0) * e1->newPosition + (1.0 / 8.0) * e2->newPosition;
		} else {
			n = v->degree(); // number of faces/edges touching vertex
			h = v->halfedge(); // get halfedge of vertex
			q = Vector3D();
			r = Vector3D();
			do {
				if (!h->face()->isBoundary()) { // check if face is a hole
					q += (*h).face()->newPosition;
					r += (*h).edge()->newPosition; // okay to be a boundary edge?
				}
				h = h->twin()->next();
			} while (h != v->halfedge());
			q *= 1.0 / n;
			r *= 1.0 / n;
			////cout << "q: " << q << endl;
			////cout << "r: " << r << endl;
			////cout << "v: " << v->position << endl;
			v->newPosition = (q + 2 * r + (n - 3) * v->position) / n;
			////cout << "vert: " << v->newPosition << endl;
		}

	}
	////cout << "vertices worked!" << endl;
}

/**
 * Assign a unique integer index to each vertex, edge, and face in
 * the mesh, starting at 0 and incrementing by 1 for each element.
 * These indices will be used as the vertex indices for a mesh
 * subdivided using Catmull-Clark (or linear) subdivision.
 */
void HalfedgeMesh::assignSubdivisionIndices() {
  // TODO Start a counter at zero; if you like, you can use the
  // "Index" type (defined in halfedgeMesh.h)
	Index pos = 0;
  // TODO Iterate over vertices, assigning values to Vertex::index
	for (VertexIter v = this->verticesBegin(); v != this->verticesEnd(); v++) {
		v->index = pos;
		pos++;
	}
  // TODO Iterate over edges, assigning values to Edge::index
	for (EdgeIter e = this->edgesBegin(); e != this->edgesEnd(); e++) {
		e->index = pos;
		pos++;
	}
  // TODO Iterate over faces, assigning values to Face::index
	for (FaceIter f = this->facesBegin(); f != this->facesEnd(); f++) {
		f->index = pos;
		pos++;
	}
  //showError("assignSubdivisionIndices() not implemented.");
}

/**
 * Build a flat list containing all the vertex positions for a
 * Catmull-Clark (or linear) subdivison of this mesh.  The order of
 * vertex positions in this list must be identical to the order
 * of indices assigned to Vertex::newPosition, Edge::newPosition,
 * and Face::newPosition.
 */
void HalfedgeMesh::buildSubdivisionVertexList(vector<Vector3D>& subDVertices) {
  // TODO Resize the vertex list so that it can hold all the vertices.

  // TODO Iterate over vertices, assigning Vertex::newPosition to the
  // appropriate location in the new vertex list.
	for (VertexIter v = this->verticesBegin(); v != this->verticesEnd(); v++) {
		subDVertices.push_back(v->newPosition);
	}

  // TODO Iterate over edges, assigning Edge::newPosition to the appropriate
  // location in the new vertex list.
	for (EdgeIter e = this->edgesBegin(); e != this->edgesEnd(); e++) {
		subDVertices.push_back(e->newPosition);
	}

  // TODO Iterate over faces, assigning Face::newPosition to the appropriate
  // location in the new vertex list.
	for (FaceIter f = this->facesBegin(); f != this->facesEnd(); f++) {
		subDVertices.push_back(f->newPosition);
	}
  //showError("buildSubdivisionVertexList() not implemented.");
}

/**
 * Build a flat list containing all the quads in a Catmull-Clark
 * (or linear) subdivision of this mesh.  Each quad is specified
 * by a vector of four indices (i,j,k,l), which come from the
 * members Vertex::index, Edge::index, and Face::index.  Note that
 * the ordering of these indices is important because it determines
 * the orientation of the new quads; it is also important to avoid
 * "bowties."  For instance, (l,k,j,i) has the opposite orientation
 * of (i,j,k,l), and if (i,j,k,l) is a proper quad, then (i,k,j,l)
 * will look like a bowtie.
 */
void HalfedgeMesh::buildSubdivisionFaceList(vector<vector<Index> >& subDFaces) {
  // TODO This routine is perhaps the most tricky step in the construction of
  // a subdivision mesh (second, perhaps, to computing the actual Catmull-Clark
  // vertex positions).  Basically what you want to do is iterate over faces,
  // then for each for each face, append N quads to the list (where N is the
  // degree of the face).  For this routine, it may be more convenient to simply
  // append quads to the end of the list (rather than allocating it ahead of
  // time), though YMMV.  You can of course iterate around a face by starting
  // with its first halfedge and following the "next" pointer until you get
  // back to the beginning.  The tricky part is making sure you grab the right
  // indices in the right order---remember that there are indices on vertices,
  // edges, AND faces of the original mesh.  All of these should get used.  Also
  // remember that you must have FOUR indices per face, since you are making a
  // QUAD mesh!

  // TODO iterate over faces
  // TODO loop around face
  // TODO build lists of four indices for each sub-quad
  // TODO append each list of four indices to face list
	vector<Index> quad(4);
	HalfedgeIter h, hNext;
	Size n;
	for (FaceIter f = this->facesBegin(); f != this->facesEnd(); f++) {
		h = f->halfedge();
		hNext = f->halfedge();
		n = f->degree() - 1;
		do {
			if (!f->isBoundary()) { // check if face is a hole
				quad[0] = (*h).vertex()->index; // original vertice
				////cout << "vert: " << (*h).vertex()->index << endl;
				quad[1] = (*h).edge()->index; // edge vertice
				////cout << "edge 1: " << (*h).edge()->index << endl;
				quad[2] = f->index; // face vertice
				////cout << "face: " << f->index << endl;
				while (n > 0) {
					hNext = hNext->next();
					n--;
				}
				quad[3] = (*hNext).edge()->index; // edge vertice
				////cout << "edge 2: " << (*hNext).edge()->index << endl;
				subDFaces.push_back(quad);
				n = f->degree();
			}
			h = h->next();
			hNext = hNext->next();
		} while (h != f->halfedge()); // keep looping until come back to first halfedge
	}
}

FaceIter HalfedgeMesh::bevelVertex(VertexIter v) {
  // *** Extra Credit ***
  // TODO This method should replace the vertex v with a face, corresponding to
  // a bevel operation. It should return the new face.  NOTE: This method is
  // responsible for updating the *connectivity* of the mesh only---it does not
  // need to update the vertex positions.  These positions will be updated in
  // HalfedgeMesh::bevelVertexComputeNewPositions (which you also have to
  // implement!)

  FaceIter newFace = this->newFace();
  HalfedgeIter h0 = v->halfedge();
  HalfedgeIter current = h0;
  bool assigned = false;
  HalfedgeIter prevNewHalfedge;
  int n = v->degree();
  for(int i = 0; i < n; i++) {
  	HalfedgeIter twin = current->twin();
  	HalfedgeIter next = twin->next();

  	VertexIter ov0 = twin->vertex();
  	FaceIter of0 = current->face();
  	FaceIter of1 = twin->face();

  	HalfedgeIter nh0 = this->newHalfedge();
  	HalfedgeIter nh1 = this->newHalfedge();
    HalfedgeIter nh2 = this->newHalfedge();
  	HalfedgeIter nh3 = this->newHalfedge();

  	VertexIter nv0 = this->newVertex();

  	EdgeIter ne0 = this->newEdge();
  	EdgeIter ne1 = this->newEdge();

    HalfedgeIter intonh1 = current;
  	while(intonh1->next() != twin) {
  		intonh1 = intonh1->next()->twin();
  	}

  	nh0->next() = current->next();
    nh0->twin() = nh1;
    nh0->vertex() = nv0;
    nh0->edge() = ne0;
    nh0->face() = of0;

    nh1->next() = nh2;
    nh1->twin() = nh0;
    nh1->vertex() = ov0;
    nh1->edge() = ne0;
    nh1->face() = of1;

    intonh1->next() = nh1;

    if(assigned) {
    	prevNewHalfedge->next() = nh0;
    }
    nh2->twin() = nh3;
    nh2->vertex() = nv0;
    nh2->edge() = ne1;
    nh2->face() = of1;

    if(assigned) {
    	nh3->next() = prevNewHalfedge->twin();
    }
    nh3->twin() = nh2;
    if(assigned) {
    	prevNewHalfedge->twin()->vertex() = nv0;
    }
    nh3->edge() = ne1;
    nh3->face() = newFace;

    ne0->halfedge() = nh0;
    ne1->halfedge() = nh2;

    nv0->halfedge() = nh0;
    ov0->halfedge() = nh1;

    of0->halfedge() = nh0;

    prevNewHalfedge = nh2;
    assigned = true;

    this->deleteEdge(current->edge());
  	this->deleteHalfedge(current);
  	this->deleteHalfedge(twin);

  	current = next;
  }

  HalfedgeIter nh0 = prevNewHalfedge->face()->halfedge();
  HalfedgeIter nh3 = nh0->twin()->next()->twin();
  prevNewHalfedge->next() = nh0;
  nh3->next() = prevNewHalfedge->twin();
  prevNewHalfedge->twin()->vertex() = nh0->vertex();
  newFace->halfedge() = prevNewHalfedge->twin();

  this->deleteVertex(v);
  return newFace;
}

FaceIter HalfedgeMesh::bevelEdge(EdgeIter e) {
  VertexIter mv0 = e->halfedge()->vertex();
  VertexIter mv1 = e->halfedge()->twin()->vertex();

  Vector3D mv0pos = mv0->position;
  Vector3D mv1pos = mv1->position;

  VertexIter v = mv0;
  FaceIter newFace = this->newFace();
  HalfedgeIter h0 = v->halfedge();
  HalfedgeIter current = h0;
  bool assigned = false;
  HalfedgeIter prevNewHalfedge;
  int n1 = mv0->degree();
  int n2 = mv1->degree();
  for(int i = 0; i < n1; i++) {
  	HalfedgeIter twin = current->twin();
  	HalfedgeIter next = twin->next();

  	VertexIter ov0 = twin->vertex();
  	FaceIter of0 = current->face();
  	FaceIter of1 = twin->face();

  	HalfedgeIter nh0 = this->newHalfedge();
  	HalfedgeIter nh1 = this->newHalfedge();
    HalfedgeIter nh2 = this->newHalfedge();
  	HalfedgeIter nh3 = this->newHalfedge();

  	VertexIter nv0 = this->newVertex();
  	nv0->position = mv0pos;

  	EdgeIter ne0 = this->newEdge();
  	EdgeIter ne1 = this->newEdge();

  	if(ov0 == mv1) {
  		mv0 = nv0;
  	}

    HalfedgeIter intonh1 = current;
  	while(intonh1->next() != twin) {
  		intonh1 = intonh1->next()->twin();
  	}

  	nh0->next() = current->next();
    nh0->twin() = nh1;
    nh0->vertex() = nv0;
    nh0->edge() = ne0;
    nh0->face() = of0;

    nh1->next() = nh2;
    nh1->twin() = nh0;
    nh1->vertex() = ov0;
    nh1->edge() = ne0;
    nh1->face() = of1;

    intonh1->next() = nh1;

    if(assigned) {
    	prevNewHalfedge->next() = nh0;
    }
    nh2->twin() = nh3;
    nh2->vertex() = nv0;
    nh2->edge() = ne1;
    nh2->face() = of1;

    if(assigned) {
    	nh3->next() = prevNewHalfedge->twin();
    }
    nh3->twin() = nh2;
    if(assigned) {
    	prevNewHalfedge->twin()->vertex() = nv0;
    }
    nh3->edge() = ne1;
    nh3->face() = newFace;

    ne0->halfedge() = nh0;
    ne1->halfedge() = nh2;

    nv0->halfedge() = nh0;
    ov0->halfedge() = nh1;

    of0->halfedge() = nh0;

    prevNewHalfedge = nh2;
    assigned = true;

    this->deleteEdge(current->edge());
  	this->deleteHalfedge(current);
  	this->deleteHalfedge(twin);

  	current = next;
  }

  HalfedgeIter nh0 = prevNewHalfedge->face()->halfedge();
  HalfedgeIter nh3 = nh0->twin()->next()->twin();
  prevNewHalfedge->next() = nh0;
  nh3->next() = prevNewHalfedge->twin();
  prevNewHalfedge->twin()->vertex() = nh0->vertex();
  newFace->halfedge() = prevNewHalfedge->twin();

  this->deleteVertex(v);

  v = mv1;
  h0 = v->halfedge();
  current = h0;
  assigned = false;
  for(int i = 0; i < n2; i++) {
  	HalfedgeIter twin = current->twin();
  	HalfedgeIter next = twin->next();

  	VertexIter ov0 = twin->vertex();
  	FaceIter of0 = current->face();
  	FaceIter of1 = twin->face();

  	HalfedgeIter nh0 = this->newHalfedge();
  	HalfedgeIter nh1 = this->newHalfedge();
    HalfedgeIter nh2 = this->newHalfedge();
  	HalfedgeIter nh3 = this->newHalfedge();

  	VertexIter nv0 = this->newVertex();
  	nv0->position = mv1pos;

  	EdgeIter ne0 = this->newEdge();
  	EdgeIter ne1 = this->newEdge();

  	if(ov0 == mv0) {
  		mv1 = nv0;
  	}

    HalfedgeIter intonh1 = current;
  	while(intonh1->next() != twin) {
  		intonh1 = intonh1->next()->twin();
  	}

  	nh0->next() = current->next();
    nh0->twin() = nh1;
    nh0->vertex() = nv0;
    nh0->edge() = ne0;
    nh0->face() = of0;

    nh1->next() = nh2;
    nh1->twin() = nh0;
    nh1->vertex() = ov0;
    nh1->edge() = ne0;
    nh1->face() = of1;

    intonh1->next() = nh1;

    if(assigned) {
    	prevNewHalfedge->next() = nh0;
    }
    nh2->twin() = nh3;
    nh2->vertex() = nv0;
    nh2->edge() = ne1;
    nh2->face() = of1;

    if(assigned) {
    	nh3->next() = prevNewHalfedge->twin();
    }
    nh3->twin() = nh2;
    if(assigned) {
    	prevNewHalfedge->twin()->vertex() = nv0;
    }
    nh3->edge() = ne1;
    nh3->face() = newFace;

    ne0->halfedge() = nh0;
    ne1->halfedge() = nh2;

    nv0->halfedge() = nh0;
    ov0->halfedge() = nh1;

    of0->halfedge() = nh0;

    prevNewHalfedge = nh2;
    assigned = true;

    this->deleteEdge(current->edge());
  	this->deleteHalfedge(current);
  	this->deleteHalfedge(twin);

  	current = next;
  }

  nh0 = prevNewHalfedge->face()->halfedge();
  nh3 = nh0->twin()->next()->twin();
  prevNewHalfedge->next() = nh0;
  nh3->next() = prevNewHalfedge->twin();
  prevNewHalfedge->twin()->vertex() = nh0->vertex();
  newFace->halfedge() = prevNewHalfedge->twin();

  this->deleteVertex(v);

  HalfedgeIter bh0 = mv0->halfedge();
  while(bh0->twin()->vertex() != mv1) {
  	bh0 = bh0->twin()->next();
  }
  HalfedgeIter bh1 = bh0->twin();

  HalfedgeIter ref0 = bh1->next()->twin();
  HalfedgeIter oh0 = ref0;
  while(oh0->next()->vertex() != ref0->vertex()) {
  	oh0 = oh0->next();
  }

  HalfedgeIter oh1 = bh1->next()->twin()->next()->next()->twin()->next()->twin();
  HalfedgeIter oh2 = bh0->next()->twin()->next()->next()->twin()->next()->twin();
  HalfedgeIter ref3 = bh0->next()->twin();
  HalfedgeIter oh3 = ref3;

  while(oh3->next()->vertex() != ref3->vertex()) {
  	oh3 = oh3->next();
  }

  HalfedgeIter oh4 = bh1->next()->next();
  HalfedgeIter oh5 = bh0->next()->next();
  HalfedgeIter oh6 = bh1->next()->twin()->next()->next();
  HalfedgeIter oh7 = bh0->next()->twin()->next()->next();

  EdgeIter etd0 = bh0->edge();
  EdgeIter etd1 = bh0->next()->edge();
  EdgeIter etd2 = bh0->next()->twin()->next()->edge();
  EdgeIter etd3 = bh1->next()->edge();
  EdgeIter etd4 = bh1->next()->twin()->next()->edge();


  this->deleteHalfedge(etd0->halfedge()->twin());
  this->deleteHalfedge(etd0->halfedge());
  this->deleteEdge(etd0);

  this->deleteHalfedge(etd1->halfedge()->twin());
  this->deleteHalfedge(etd1->halfedge());
  this->deleteEdge(etd1);

  this->deleteHalfedge(etd2->halfedge()->twin());
  this->deleteHalfedge(etd2->halfedge());
  this->deleteEdge(etd2);

  this->deleteHalfedge(etd3->halfedge()->twin());
  this->deleteHalfedge(etd3->halfedge());
  this->deleteEdge(etd3);

  this->deleteHalfedge(etd4->halfedge()->twin());
  this->deleteHalfedge(etd4->halfedge());
  this->deleteEdge(etd4);

  this->deleteVertex(mv0);
  this->deleteVertex(mv1);

  EdgeIter ned0 = this->newEdge();
  EdgeIter ned1 = this->newEdge();

  HalfedgeIter nhe0 = this->newHalfedge();
  HalfedgeIter nhe1 = this->newHalfedge();
  HalfedgeIter nhe2 = this->newHalfedge();
  HalfedgeIter nhe3 = this->newHalfedge();

  oh0->next() = nhe1;
  oh1->next() = nhe3;
  oh2->next() = nhe0;
  oh3->next() = nhe2;

  nhe0->next() = oh4;
  nhe0->twin() = nhe1;
  nhe0->vertex() = oh2->twin()->vertex();
  nhe0->edge() = ned0;
  nhe0->face() = oh2->face();

  nhe1->next() = oh7;
  nhe1->twin() = nhe0;
  nhe1->vertex() = oh0->twin()->vertex();//
  nhe1->edge() = ned0;
  nhe1->face() = newFace;

  nhe2->next() = oh6;
  nhe2->twin() = nhe3;
  nhe2->vertex() = oh3->twin()->vertex();//
  nhe2->edge() = ned1;
  nhe2->face() = newFace;

  nhe3->next() = oh5;
  nhe3->twin() = nhe2;
  nhe3->vertex() = oh1->twin()->vertex();
  nhe3->edge() = ned1;
  nhe3->face() = oh1->face();

  oh4->face()->halfedge() = oh4;
  oh2->twin()->face()->halfedge() = oh2->twin();
  oh5->face()->halfedge() = oh5;
  oh1->twin()->face()->halfedge() = oh1->twin();

  ned0->halfedge() = nhe2;
  ned1->halfedge() = nhe1;

  newFace->halfedge() = oh0;
  return newFace;
}

FaceIter HalfedgeMesh::bevelFace(FaceIter f) {
  // *** Extra Credit ***
  // TODO This method should replace the face f with an additional, inset face
  // (and ring of faces around it), corresponding to a bevel operation. It
  // should return the new face.  NOTE: This method is responsible for updating
  // the *connectivity* of the mesh only---it does not need to update the vertex
  // positions.  These positions will be updated in
  // HalfedgeMesh::bevelFaceComputeNewPositions (which you also have to
  // implement!)


  vector<HalfedgeIter> newHalfEdges;
  FaceIter newFace = this->newFace();

  HalfedgeIter h0 = f->halfedge();
  HalfedgeIter current = h0;

  bool assigned = false;
  HalfedgeIter prevNewHalfedge;
  VertexIter prevNewVertex;
  do {
  	HalfedgeIter next = current->next();


    HalfedgeIter nh0 = this->newHalfedge();
    HalfedgeIter nh1 = this->newHalfedge();
    HalfedgeIter nh2 = this->newHalfedge();
    HalfedgeIter nh3 = this->newHalfedge();

    VertexIter nv0 = this->newVertex();

    EdgeIter ne0 = this->newEdge();
    EdgeIter ne1 = this->newEdge();

    FaceIter nf0 = this->newFace();

    nh0->next() = nh1;
    nh0->twin() = nh3;
    nh0->vertex() = next->vertex();
    nh0->edge() = ne0;
    nh0->face() = nf0;

    if(assigned) {
    	nh1->next() = prevNewHalfedge;
    }
    nh1->twin() = nh2;
    nh1->vertex() = nv0;
    nh1->edge() = ne1;
    nh1->face() = nf0;

    nh2->twin() = nh1;
    if(assigned) {
      nh2->vertex() = prevNewVertex;
    }
    nh2->edge() = ne1;
    nh2->face() = newFace;

    if(assigned) {
      prevNewHalfedge->next() = current;
      prevNewHalfedge->face() = nf0;
    }

    nh3->twin() = nh0;
    nh3->vertex() = nv0;
    nh3->edge() = ne0;

    current->next() = nh0;
    current->face() = nf0;

    nv0->halfedge() = nh3;

    ne0->halfedge() = nh0;
    ne1->halfedge() = nh1;

    if(assigned) {
    	nf0->halfedge() = prevNewHalfedge;
    }


    prevNewVertex = nv0;
    prevNewHalfedge = nh3;

    current = next;
    if(assigned) {
    	nh1->next()->twin()->next()->twin()->next() = nh2;
    }
    assigned = true;
  }while(current != h0);
  HalfedgeIter nh0 = current->next();
  nh0->next()->next() = prevNewHalfedge;
  nh0->next()->twin()->vertex() = prevNewVertex;
  prevNewHalfedge->next() = current;
  prevNewHalfedge->face() = nh0->face();
  nh0->next()->next()->twin()->next()->twin()->next() = nh0->next()->twin();
  nh0->face()->halfedge() = prevNewHalfedge;
  newFace->halfedge() = nh0->next()->twin();
  this->deleteFace(f);
  return newFace;
}


void HalfedgeMesh::bevelFaceComputeNewPositions(
    vector<Vector3D>& originalVertexPositions,
    vector<HalfedgeIter>& newHalfedges, double normalShift,
    double tangentialInset) {
  // *** Extra Credit ***
  // TODO Compute new vertex positions for the vertices of the beveled face.
  //
  // These vertices can be accessed via newHalfedges[i]->vertex()->position for
  // i = 1, ..., newHalfedges.size()-1.
  //
  // The basic strategy here is to loop over the list of outgoing halfedges,
  // and use the preceding and next vertex position from the original mesh
  // (in the originalVertexPositions array) to compute an offset vertex
  // position.
  //
  // Note that there is a 1-to-1 correspondence between halfedges in
  // newHalfedges and vertex positions
  // in orig.  So, you can write loops of the form
  //
  // for( int i = 0; i < newHalfedges.size(); hs++ )
  // {
  //    Vector3D pi = originalVertexPositions[i]; // get the original vertex
  //    position correponding to vertex i
  // }
  //
  // Get the number of vertices in the new polygon
	int N = newHalfedges.size();

	// Assuming we're looking at vertex i, compute the indices
	// of the next and previous elements in the list using
	// modular arithmetic---note that to get the previous index,
	// we can't just subtract 1 because the mod operation in C++
	// doesn't behave quite how you might expect for negative
	// values!


	// Get the actual 3D vertex coordinates at these vertices

	for(int i = 0; i < N; i++) {
		int a = (i+N-1) % N;
		int b = i;
		int c = (i+1) % N;
		Vector3D pa = originalVertexPositions[a];
		Vector3D pb = originalVertexPositions[b];
		Vector3D pc = originalVertexPositions[c];

		if(newHalfedges[a]->vertex()->position == newHalfedges[b]->vertex()->position && newHalfedges[b]->vertex()->position == newHalfedges[c]->vertex()->position && newHalfedges[a]->vertex()->position == newHalfedges[c]->vertex()->position) {
		  for(int j = 0; j < N; j++) {
		  	int d = (j+N-1) % N;
			int e = j;
			int f = (j+1) % N;
			Vector3D pd = originalVertexPositions[d];
			Vector3D pe = originalVertexPositions[e];
			Vector3D pf = originalVertexPositions[f];
			newHalfedges[j]->vertex()->position = pe + (((pd - pe) + (pf - pe)) * .1f);
		  }
	    }
		newHalfedges[i]->vertex()->position += newHalfedges[i]->twin()->next()->twin()->face()->normal() * normalShift;
	}

}

void HalfedgeMesh::bevelVertexComputeNewPositions(
    Vector3D originalVertexPosition, vector<HalfedgeIter>& newHalfedges,
    double tangentialInset) {
    int N = newHalfedges.size();

    double t = .1;

	// Assuming we're looking at vertex i, compute the indices
	// of the next and previous elements in the list using
	// modular arithmetic---note that to get the previous index,
	// we can't just subtract 1 because the mod operation in C++
	// doesn't behave quite how you might expect for negative
	// values!


	// Get the actual 3D vertex coordinates at these vertices
	bool same = true;
    for(int i = 1; i < N; i++) {
		if(!(newHalfedges[i]->vertex()->position == newHalfedges[i-1]->vertex()->position)) {
			same = false;
			break;
		}
	}
	if(same) {
		for(int i = 0; i < N; i++) {
		   newHalfedges[i]->vertex()->position = originalVertexPosition;
		}
	}

	for(int i = 0; i < N; i++) {
		Vector3D dto = (originalVertexPosition - newHalfedges[i]->vertex()->position);
		double distanceMagnitude = sqrt(dto.x * dto.x + dto.y * dto.y + dto.z * dto.z);
		Vector3D dtr = (originalVertexPosition - newHalfedges[i]->twin()->vertex()->position);
		double referenceMagnitude = sqrt(dtr.x * dtr.x + dtr.y * dtr.y + dtr.z * dtr.z);
        double t = distanceMagnitude / referenceMagnitude;
        t -= tangentialInset;
        if(t > 1) {
        	t = 1;
        }
        if(t < 0) {
        	t = 0;
        }
		newHalfedges[i]->vertex()->position = originalVertexPosition * (1 - t) + newHalfedges[i]->twin()->vertex()->position * t;
	}

}

void HalfedgeMesh::bevelEdgeComputeNewPositions(
    vector<Vector3D>& originalVertexPositions,
    vector<HalfedgeIter>& newHalfedges, double tangentialInset) {

  // *** Extra Credit ***
  // TODO Compute new vertex positions for the vertices of the beveled edge.
  //
  // These vertices can be accessed via newHalfedges[i]->vertex()->position for
  // i = 1, ..., newHalfedges.size()-1.
  //
  // The basic strategy here is to loop over the list of outgoing halfedges,
  // and use the preceding and next vertex position from the original mesh
  // (in the orig array) to compute an offset vertex position.
  //
  // Note that there is a 1-to-1 correspondence between halfedges in
  // newHalfedges and vertex positions
  // in orig.  So, you can write loops of the form
  //
  // for( int i = 0; i < newHalfedges.size(); i++ )
  // {
  //    Vector3D pi = originalVertexPositions[i]; // get the original vertex
  //    position correponding to vertex i
  // }
  //V
	int N = newHalfedges.size();
	for(int i = 0; i < N; i++) {
		Vector3D originalVertexPosition = originalVertexPositions[i];
		Vector3D dto = (originalVertexPosition - newHalfedges[i]->vertex()->position);
		double distanceMagnitude = sqrt(dto.x * dto.x + dto.y * dto.y + dto.z * dto.z);
		Vector3D dtr = (originalVertexPosition - newHalfedges[i]->twin()->vertex()->position);
		double referenceMagnitude = sqrt(dtr.x * dtr.x + dtr.y * dtr.y + dtr.z * dtr.z);
        double t = distanceMagnitude / referenceMagnitude;
        t -= tangentialInset;
        if(t > 1) {
        	t = 1;
        }
        if(t < 0) {
        	t = 0;
        }
        newHalfedges[i]->vertex()->position = originalVertexPosition * (1 - t) + newHalfedges[i]->twin()->vertex()->position * t;
	}
}

void HalfedgeMesh::splitPolygons(vector<FaceIter>& fcs) {
  for (auto f : fcs) splitPolygon(f);
}

void HalfedgeMesh::splitPolygon(FaceIter f) {
  if(f->degree() <= 3) {
  	return;
  } else {
  	HalfedgeIter h = f->halfedge();
  	HalfedgeIter next = h->next();
  	HalfedgeIter prev = next;
  	while(prev->next() != h) {
  		prev = prev->next();
  	}
  	EdgeIter ne0 = this->newEdge();
  	HalfedgeIter nh0 = this->newHalfedge();
  	HalfedgeIter nh1 = this->newHalfedge();
  	FaceIter nf0 = this->newFace();
  	nf0->halfedge() = h;
  	ne0->halfedge() = nh0;
  	f->halfedge() = next->next();

  	nh0->next() = h;
  	nh0->twin() = nh1;
    nh0->vertex() = next->next()->vertex();
    nh0->edge() = ne0;
    nh0->face() = nf0;

    nh1->next() = next->next();
    nh1->twin() = nh0;
    nh1->vertex() = h->vertex();
    nh1->edge() = ne0;
    nh1->face() = f;

    prev->next() = nh1;
    next->next() = nh0;
    splitPolygon(f);

  }
}

EdgeRecord::EdgeRecord(EdgeIter& _edge) : edge(_edge) {
  // *** Extra Credit ***
  // TODO: (meshEdit)
  // Compute the combined quadric from the edge endpoints.
  // -> Build the 3x3 linear system whose solution minimizes the quadric error
  //    associated with these two endpoints.
  // -> Use this system to solve for the optimal position, and store it in
  //    EdgeRecord::optimalPoint.
  // -> Also store the cost associated with collapsing this edg in
  //    EdgeRecord::Cost.
  Matrix4x4 K_ij = _edge->halfedge()->vertex()->quadric + _edge->halfedge()->twin()->vertex()->quadric;
  cout << "K_ij\n: " << K_ij << endl;
  Matrix3x3 A;
  Vector3D b;
  for(int i = 0; i < 3; i++) {
  	for(int j = 0; j < 3; j++) {
  		A(i, j) = K_ij(i, j);
  	}
  }
  /*
  b.x = K_ij(0, 3);
  b.y = K_ij(1, 3);
  b.z = K_ij(2, 3);
  */
  b.x = K_ij(0, 3);
  b.y = K_ij(1, 3);
  b.z = K_ij(2, 3);

  cout << "A\n: " << A << endl;
  cout << "b: " << b << endl;
  Vector3D x = A.inv() * b;

  cout << "A Det: " << A.det() << endl;
  cout << "A inv:\n " << A.inv() << endl;
  cout << "x:\n " << x << endl;

  Vector4D u;
  u.x = x.x;
  u.y = x.y;
  u.z = x.z;
  u.w = 1;

  Matrix4x4 u_T;
  u_T.zero(0);
  u_T.column(3) = u;
  u_T = u_T.T();

  Vector4D prod = (u_T * K_ij).T().column(3);
  cout << "STUFF: " << endl;
  cout << u_T << "\n" << K_ij << "\n" << u << endl;

  double cost = prod.x * u.x + prod.y * u.y + prod.z * u.z + prod.w * u.w;
  cout << "New Cost: " << cost << endl;
  this->optimalPoint = x;
  this->score = cost;
  _edge->record = *this;
}

void MeshResampler::upsample(HalfedgeMesh& mesh)
// This routine should increase the number of triangles in the mesh using Loop
// subdivision.
{
  // TODO: (meshEdit)
  // Compute new positions for all the vertices in the input mesh, using
  // the Loop subdivision rule, and store them in Vertex::newPosition.
  // -> At this point, we also want to mark each vertex as being a vertex of the
  //    original mesh.

	/*
	map<HalfedgeIter, double> vertices;
	map<HalfedgeIter, double>::iterator index;
	Vector3D s;
	*/
    for (EdgeIter e = mesh.edgesBegin(); e != mesh.edgesEnd(); e++) {
    	e->isNew = false;
		if(e->isBoundary()) {
			e->newPosition = e->centroid();
		} else {
			Vector3D s;
			 // original
			s += e->halfedge()->vertex()->position * (3.0 / 8.0);
			s += e->halfedge()->twin()->vertex()->position * (3.0 / 8.0);
			s += e->halfedge()->next()->next()->vertex()->position * (1.0 / 8.0);
			s += e->halfedge()->twin()->next()->next()->vertex()->position * (1.0 / 8.0);
			e->newPosition = s;
		}

	}
	//cout << "edge positions done!" << endl;

	for (VertexIter v = mesh.verticesBegin(); v != mesh.verticesEnd(); v++) {

		v->isNew = false; // original
		if(v->isBoundary()) {
			HalfedgeIter h = v->halfedge();
			EdgeIter e1, e2;
			int foundEdges = 0;
			do {
	          if(h->edge()->isBoundary()) {
	          	if(foundEdges == 0) {
	          		e1 = h->edge();
	          	} else {
	          		e2 = h->edge();
	          	}
	          	foundEdges++;
	          }
	          h = h->twin()->next();
			}while(foundEdges < 2);
			v->newPosition = (3.0 / 4.0) * v->position + (1.0 / 8.0) * e1->newPosition + (1.0 / 8.0) * e2->newPosition;
		} else {
			Size n;
		    double u;
			Vector3D s;
			n = v->degree();
			if (n == 3)
				u = 3.0 / 16;
			else {
				u = 3.0 / (8 * n);
			}
			//v->getNeighborhood(vertices); // gets all neighboring vertices
			HalfedgeIter firstEdge = v->halfedge();
			HalfedgeIter currentEdge = firstEdge;
			do {
				currentEdge = currentEdge->twin();
			    s += currentEdge->vertex()->position;
			    currentEdge = currentEdge->next();
			}while(currentEdge != firstEdge);
			v->newPosition = ((1 - (n * u)) * v->position) + (u * s);
		}
		//cout << v->newPosition << endl;
	}
	//cout << "new vertices done!" << endl;
  // -> Next, compute the updated vertex positions associated with edges, and
  //    store it in Edge::newPosition.

  // -> Next, we're going to split every edge in the mesh, in any order.  For
  //    future reference, we're also going to store some information about which
  //    subdivided edges come from splitting an edge in the original mesh, and
  //    which edges are new, by setting the flat Edge::isNew. Note that in this
  //    loop, we only want to iterate over edges of the original mesh.
  //    Otherwise, we'll end up splitting edges that we just split (and the
  //    loop will never end!)
	/*int num = mesh.nEdges();
	EdgeIter edg = mesh.edgesBegin();
	VertexIter vNew;
	EdgeIter nextEdge;
	for (int i = 0; i < num; i++) {
		if (!vertices.empty())
			vertices.clear();
		// get the next edge NOW!
		nextEdge = edg;
		nextEdge++;

		if (!edg->isNew) {
			vNew = mesh.splitEdge(edg); // not working........?
			vNew->isNew = true;
			vNew->position = edg->newPosition;
			//cout << "edg: " << (*edg).newPosition << endl;
			//cout << "vNew: " << (*vNew).position << endl;
		}

		// mark each edge as new
		vNew->getNeighborhood(vertices);
		for (index = vertices.begin(); index != vertices.end(); index++) { // iterate over neighboring vertices
			(*index).first->edge()->isNew = true;
		}
		edg = nextEdge;
	}*/
	int n = mesh.nEdges();
	EdgeIter e = mesh.edgesBegin();
	for (int i = 0; i < n; i++) {

	  // get the next edge NOW!
	  EdgeIter nextEdge = e;
	  nextEdge++;

	  // now, even if splitting the edge deletes it...
	  if (!e->isNew) {
	  	if(!e->isBoundary()) {
			Vector3D newPos = e->newPosition;
		    VertexIter newV = mesh.splitEdge(e);
		    newV->halfedge()->edge()->isNew = false;
		    newV->halfedge()->twin()->next()->edge()->isNew = true;
		    newV->halfedge()->twin()->next()->twin()->next()->edge()->isNew = false;
		    newV->halfedge()->twin()->next()->twin()->next()->twin()->next()->edge()->isNew = true;
		    newV->isNew = true;
		    newV->newPosition = newPos;
	  	} else {
			Vector3D newPos = e->newPosition;
		    VertexIter newV = mesh.splitEdge(e);
		    HalfedgeIter newEdge = newV->halfedge();
		    do {
   				if(newEdge->edge()->isBoundary()) {
   					newEdge->edge()->isNew = false;
   				} else {
   					newEdge->edge()->isNew = true;
   				}
   				newEdge = newEdge->twin()->next();
		    }while(newEdge != newV->halfedge());
		    newV->isNew = true;
		    newV->newPosition = newPos;
	  	}

	  }

	  // ...we still have a valid reference to the next edge.
	  e = nextEdge;
	}

	////cout << "edge split done!" << endl;

  for (VertexIter v = mesh.verticesBegin(); v != mesh.verticesEnd(); v++) {
    ////cout << "Position: " << v->position << "New: " << v->isNew << endl;
  }

  for (EdgeIter ed = mesh.edgesBegin(); ed != mesh.edgesEnd(); ed++) {
	////cout << "Edge: " << ed->centroid() << "New: " << ed->isNew << endl;
  }
  // -> Now flip any new edge that connects an old and new vertex.
	// call edge flip: mesh.flipEdge(edge); if one vertice = new and other is old

	for (EdgeIter ed = mesh.edgesBegin(); ed != mesh.edgesEnd(); ed++) {
		if (ed->halfedge()->vertex()->isNew != ed->halfedge()->twin()->vertex()->isNew && ed->isNew) {
			//cout << "Flipped edge that is " << ed->isNew << "connecting " << ed->halfedge()->vertex()->position << " which is " << ed->halfedge()->vertex()->isNew << " and " << ed->halfedge()->twin()->vertex()->position << " which is " << ed->halfedge()->twin()->vertex()->isNew << endl;
		    mesh.flipEdge(ed);
		}
	}
	////cout << "edge flip done!" << endl;
  // -> Finally, copy the new vertex positions into final Vertex::position.
	// v->position = newposition;
	for (VertexIter vert = mesh.verticesBegin(); vert != mesh.verticesEnd(); vert++) {
		vert->position = vert->newPosition;
	}
	////cout << "All done!" << endl;
  // Each vertex and edge of the original surface can be associated with a
  // vertex in the new (subdivided) surface.
  // Therefore, our strategy for computing the subdivided vertex locations is to
  // *first* compute the new positions
  // using the connectity of the original (coarse) mesh; navigating this mesh
  // will be much easier than navigating
  // the new subdivided (fine) mesh, which has more elements to traverse.  We
  // will then assign vertex positions in
  // the new mesh based on the values we computed for the original mesh.

  // Compute updated positions for all the vertices in the original mesh, using
  // the Loop subdivision rule.

  // Next, compute the updated vertex positions associated with edges.

  // Next, we're going to split every edge in the mesh, in any order.  For
  // future
  // reference, we're also going to store some information about which
  // subdivided
  // edges come from splitting an edge in the original mesh, and which edges are
  // new.
  // In this loop, we only want to iterate over edges of the original
  // mesh---otherwise,
  // we'll end up splitting edges that we just split (and the loop will never
  // end!)

  // Finally, flip any new edge that connects an old and new vertex.

  // Copy the updated vertex positions to the subdivided mesh.
  //showError("upsample() not implemented.");
}

void MeshResampler::downsample(HalfedgeMesh& mesh) {
  // *** Extra Credit ***
  // TODO: (meshEdit)
  // Compute initial quadrics for each face by simply writing the plane equation
  // for the face in homogeneous coordinates. These quadrics should be stored
  // in Face::quadric
  // -> Compute an initial quadric for each vertex as the sum of the quadrics
  //    associated with the incident faces, storing it in Vertex::quadric
  // -> Build a priority queue of edges according to their quadric error cost,
  //    i.e., by building an EdgeRecord for each edge and sticking it in the
  //    queue.
  // -> Until we reach the target edge budget, collapse the best edge. Remember
  //    to remove from the queue any edge that touches the collapsing edge
  //    BEFORE it gets collapsed, and add back into the queue any edge touching
  //    the collapsed vertex AFTER it's been collapsed. Also remember to assign
  //    a quadric to the collapsed vertex, and to pop the collapsed edge off the
  //    top of the queue.
  int numFaces = mesh.nFaces();
  for(FaceIter f = mesh.facesBegin(); f != mesh.facesEnd(); f++) {
    Vector4D v;
    Vector3D normal = f->normal();
    Vector3D p = f->halfedge()->vertex()->position;
    v.x = normal.x;
    v.y = normal.y;
    v.z = normal.z;
    v.w = -1 * (normal.x * p.x + normal.y * p.y + normal.z * p.z);
    //cout << "V: " << v << endl;
    Matrix4x4 _K = outer(v, v);
    //cout << "K: \n" << _K << endl;
    f->quadric = _K;
  }
  for(VertexIter v = mesh.verticesBegin(); v != mesh.verticesEnd(); v++) {
  	Matrix4x4 K_i;
  	HalfedgeIter h = v->halfedge();
  	HalfedgeIter current = h;
  	do {
  	  K_i += current->face()->quadric;
      h = h->twin()->next();
  	}while(current != h);
  	v->quadric = K_i;
  }
  MutablePriorityQueue<EdgeRecord> queue;
  for(EdgeIter e = mesh.edgesBegin(); e != mesh.edgesEnd(); e++) {
  	EdgeRecord myRecord(e);
    queue.insert(myRecord);
  }
  while(mesh.nFaces() > numFaces - 1) {

  	//cout << "Alive1" << endl;
  	EdgeRecord cheapest = queue.top();
  	//cout << "Alive1" << endl;
  	queue.pop();
  	//cout << "Alive1" << endl;
  	Matrix4x4 newQuadric = cheapest.edge->halfedge()->vertex()->quadric + cheapest.edge->halfedge()->twin()->vertex()->quadric;
    //cout << "Alive1" << endl;

    HalfedgeIter start = cheapest.edge->halfedge();
    //cout << "Alive1" << endl;
    HalfedgeIter current = start;
    do {
      queue.remove(current->edge()->record);
      current = current->twin()->next();
      //cout << "Finiteness Verification 1" << endl;
    }while(current != start);

    start = cheapest.edge->halfedge()->twin();
    current = start;
    do {
      queue.remove(current->edge()->record);
      current = current->twin()->next();
      //cout << "Finiteness Verification 2" << endl;
    }while(current != start);
    int b4 = mesh.nFaces();
    Vector3D artificialEstimate = (cheapest.edge->halfedge()->vertex()->position + cheapest.edge->halfedge()->twin()->vertex()->position) / 2.;
    cout << "Boutta collapse an edge connecting " << cheapest.edge->halfedge()->vertex()->position << " and " << cheapest.edge->halfedge()->twin()->vertex()->position << endl;
    //cout << "Test: " << cheapest.edge->halfedge()->edge()->halfedge()->vertex()->position << endl;
    VertexIter newV = mesh.collapseEdge(cheapest.edge);
    cout << "Cost: " << cheapest.score << endl;
    //cout << " COLLAPSED ONE EDGE CONNECTING " << cheapest.edge->halfedge()->vertex()->position << " and " << cheapest.edge->halfedge()->twin()->vertex()->position << endl;
    newV->quadric = newQuadric;
    //newV->position = cheapest.optimalPoint;
    newV->position = artificialEstimate;

    start = newV->halfedge();
    current = start;
    do {
      queue.insert(EdgeRecord(current->edge()));
      current = current->twin()->next();
      //cout << "Finiteness Verification 3" << endl;
    }while(current != start);
    //cout << "Finished." << endl;
  }
}

void MeshResampler::resample(HalfedgeMesh& mesh) {
    double mean = 0;
	for (EdgeIter e = mesh.edgesBegin(); e != mesh.edgesEnd(); e++) {
		mean += e->length() / mesh.nEdges();
	}
	cout << "average edge: " << mean << endl;
	// Repeat the four main steps for 5 or 6 iterations
	// -> Split edges much longer than the target length (being careful about
	//    how the loop is written!)
	int n = mesh.nEdges();
	EdgeIter e = mesh.edgesBegin();
	EdgeIter nextEdge;
	for (int i = 0; i < n; i++) {
		nextEdge = e;
		nextEdge++;
		if (e->length() > 4 * mean / 3) {
			mesh.splitEdge(e);
		}
		e = nextEdge;
	}
	cout << "edge splits done" << endl;

	// -> Collapse edges much shorter than the target length.  Here we need to
	//    be EXTRA careful about advancing the loop, because many edges may have
	//    been destroyed by a collapse (which ones?)
	// iterate over all edges in the mesh

	/*n = mesh.nEdges();
	e = mesh.edgesBegin();
	HalfedgeIter h;
	FaceIter f0, f1;
	for (int i = 0; i < n; i++) {
		nextEdge = e;
		nextEdge++;
		if (e->length() < 4 * mean / 5) {
			cout << "collapse executing!" << endl;
			h = e->halfedge();
			f0 = h->face();
			f1 = h->twin()->face();
			mesh.collapseEdge(e);
			if (f0->degree() == 3)
				i--; // extra edge eliminated
			if (f1->degree() == 3)
				i--; // extra edge eliminated
		}
		e = nextEdge;
	}*/

	cout << "edge collapse done" << endl;
	// -> Now flip each edge if it improves vertex degree
	int before, after;
	int a1, a2, b1, b2;
	for (EdgeIter e = mesh.edgesBegin(); e != mesh.edgesEnd(); e++) {
		a1 = e->halfedge()->vertex()->degree();
		a2 = e->halfedge()->twin()->vertex()->degree();
		b1 = e->halfedge()->next()->next()->vertex()->degree();
		b2 = e->halfedge()->twin()->next()->next()->vertex()->degree();
		before = abs(a1 - 6) + abs(a2 - 6) + abs(b1 - 6) + abs(b2 - 6);
		after = abs(a1 - 7) + abs(a2 - 7) + abs(b1 - 5) + abs(b2 - 5);
		if (before > after) {
			mesh.flipEdge(e);
		}
	}
	cout << "edge flip done" << endl;

	Vector3D dir;
	// -> Finally, apply some tangential smoothing to the vertex positions
	for (VertexIter v = mesh.verticesBegin(); v != mesh.verticesEnd(); v++) {
		v->newPosition = v->neighborhoodCentroid();
	}

	cout << "setting new position done" << endl;

	for (VertexIter vert = mesh.verticesBegin(); vert != mesh.verticesEnd(); vert++) {
		dir = vert->newPosition - vert->position;
		// adjusting v...
		dir = dir - dot(vert->normal(), dir) * vert->normal();
		vert->position = vert->position + 0.2 * dir;
	}

	cout << "moving to new position done" << endl;
	}

}  // namespace CS248