#include <stdlib.h>
#include "subdobj.h"
#include "scanner.h"
#include "material.h"
#include "wavefront_reader_lite.h"
#include "vertexlist.h"

void SubdObj::CopyFrom(const SubdObj&) {

	cerr << "Error!  Don't copy SubdObjs!" << endl;
	cerr << ((SubdObj *) (NULL))->_vertex_list;
}

SubdObj& SubdObj::Destroy() {

	if (_ref_count) {
		cerr << "Error!  SubdObj::Destroy() called with a non-zero _ref_count!"
			 << endl;
		exit(-1);
	}

	{
		HalfEdge *last = _half_edge_list, *next;

		while (last) {
			next = last->_next;
			delete last;
			last = next;
		}
	}

	{
		Vertex *last = _vertex_list, *next;

		while (last) {
//			cerr << "SubdObj::Destroy(): deleting vertex" << endl;
			next = last->_next;
			last->_next = NULL;
			delete last;
			last = next;
		}
	}

	_half_edge_list = _he_tail = NULL;
	_vertex_list = _v_tail = NULL;

	return *this;
}

int Hash(int i1, int i2, int size) {

	int hash;

	if (i2 > i1) {
		int tmp = i1;
		i1 = i2;
		i2 = tmp;
	}

	hash = i1 * 239 + i2 * 113 + i1 * i2;
	hash = hash % size;
	if (hash < 0)
		hash += size;

	return hash;
}

void SubdObj::PrintHashDistribution() {

	for (int i=0; i<_hash_table_size; i++) {
		int count = 0;
		for (HalfEdge *he = _loading_hash_table[i]; he; he=he->_child)
			count++;
		if (count)
			cerr << i << ": " << count << endl;
	}
}

ostream& operator<<(ostream &co, const SubdObj&) {

	co << "SubdObj!!!" << endl;

	return co;
}

istream& operator>>(istream &ci, SubdObj &SDO) {

	int i,maxverts, v;
	real x, y, z;
	char c;
	int ok;

	SDO.Destroy();

	ci >> maxverts;

	if (maxverts < 3) {
		cerr << "Error!  Must define at least 3 verticies!" << endl;
		exit(-1);
	}

	SDO._hash_table_size = maxverts * 2 + 1;
	SDO._loading_hash_table = new HalfEdgePtr[SDO._hash_table_size];

	for (i=0; i<SDO._hash_table_size; i++)
		SDO._loading_hash_table[i] = NULL;

	VertexPtr *verts = new VertexPtr[maxverts];

	ok = 1;
	i=0;

	while (ok) {

		ci >> c;

		if (c == 'V') {
			ci >> v;
			ci >> x >> y >> z;
			verts[v-1] = SDO.NewVertex(Vec4(x, y, z), 0);
			verts[v-1]->_flag = v;
			i++;
		}

		else if (c == 'S') {
			ci >> x >> y >> z;
			for (Vertex *vtx = SDO._vertex_list; vtx; vtx=vtx->_next)
				//vtx->Point() *= vec;
				vtx->Point().Vec4FastScale3(x, y, z);
		}

		else if (c == 'T') {
			ci >> x >> y >> z;
			Vec4 vec(x,y,z);
			for (Vertex *vtx = SDO._vertex_list; vtx; vtx=vtx->_next)
				Vec4FastAdd(vtx->Point(), vtx->Point(), vec);
		}

		else if (c == 'E') {
			if (i != maxverts) {
				cerr << "Warning!  Not enough vertecies!  " << maxverts
					 << " expected, got " << i << endl;
			}
			ok = 0;
		}

		else {
			cerr << "Error: Either V, S, T, or E expected" << endl;
			cerr << "Got: " << c << endl;
			exit(-1);
		}
	}

	VertexPtr *vertlist = new VertexPtr[20];
	int size;
	ok = 1;

	int fcount = 0;

	while (ok) {

		ci >> c;

		if (c == 'F') {
			fcount++;
			/* cout << "Reading face " << fcount << endl; */
			size = 0;
			v = 999;
			while (v) {
				ci >> v;
				if (v)
					vertlist[size++] = verts[v-1];
			}
			SDO.AddFace(vertlist, size);
		}

		else if (c == 'E')
			ok = 0;

		else {
			cerr << "Error!  E expected at end" << endl;
			exit(-1);
		}
	}

	delete verts;

	delete [] SDO._loading_hash_table;
	SDO._loading_hash_table = NULL;

	SDO.ClearChildren();

	return ci;
}

SubdObj& SubdObj::ReadFromFile(const JLString &filename) {

	ifstream iFile(filename.GetString(), ios::in);

	if (iFile) {
		iFile >> *this;
		iFile.close();
	}

	else
		cerr << "Error: File " << filename << " not found!" << endl;

	return *this;
}

int MustBe(const JLString &s1, Scanner &SC) {

	SC.GetNextToken();

	if (SC.GetToken() != s1) {
		cerr << "Error: Expected: " << s1 << ", got: " << SC.GetToken() << endl;
		return 0;
	}

	return 1;
}

SubdObj& SubdObj::OutputInventor(const JLString &filename) {

	ofstream oFile(filename.GetString());

	if (!oFile) {
		cerr << "Error: couldn't open file for output: " << filename << endl;
		return *this;
	}

	oFile << "#Inventor V2.0 ascii" << endl
		  << endl
		  << "Separator {" << endl
		  << "	Coordinate3 {" << endl
		  << "		point [" << endl;

	int i=0;
	for (Vertex *v = _vertex_list; v; v=v->_next) {
		v->_flag = i++;
		oFile << "			" << v->GetPoint().x() << " "
							  << v->GetPoint().y() << " "
							  << v->GetPoint().z() << "," << endl;
	}

	oFile << "		]" << endl
		  << "	}" << endl
		  << endl
		  << "	IndexedFaceSet {" << endl
		  << "		coordIndex [" << endl;

	ClearHalfEdgeFlags();

	int vertlist[255];
	int numverts;

	for (HalfEdge *he = _half_edge_list; he; he = he->_next)
		if (!he->_flag) {
			numverts = 0;
			HalfEdge *loop = he;
			do {
				vertlist[numverts++] = loop->_v->_flag;
				loop->_flag = 1;
				loop = loop->_link->_other_half;
			} while (loop != he);

			// Print vertex indecies in reverse order:
			oFile << "			";
			while (numverts > 0)
				oFile << vertlist[--numverts] << ", ";
			oFile << "-1," << endl;
		}

	oFile << "		]" << endl
		  << "	}" << endl
		  << "}" << endl;

	oFile.close();

	return *this;
}


typedef struct This_and_VL {
	SubdObj		*This;
	VertexList	*VL;
	int			face_count;
} This_and_VL;

static int VertexCallback(int i, float x, float y, float z, void *data_vcb) {

	This_and_VL *TaVL = (This_and_VL*) data_vcb;
	SubdObj *This = TaVL->This;
	VertexList *VL = TaVL->VL;

	Vertex *v = This->NewVertex(Vec4(x, y, z), 1);
	VL->PlaceInList(v);
	v->SetFlag(i);

	return 0;
}

static int FaceCallback(int n, 
						const int *vindex, const int *tindex,
						const float *v, const float *t,
						void *data_fcb) {

	This_and_VL *TaVL = (This_and_VL*) data_fcb;
	SubdObj *This = TaVL->This;
	VertexList *VL = TaVL->VL;

	VertexPtr vertlist[500];

	// modified to make faces really ccw:
	for (int i=0; i<n; i++)
		vertlist[i] = &((*VL)[vindex[n-i-1]]);

	This->AddFace(vertlist, n);

	TaVL->face_count++;
	if (!(TaVL->face_count % 1000))
		cerr << "Read face " << TaVL->face_count << endl;

	return 0;
}


SubdObj& SubdObj::ReadWavefrontFile(const JLString &filename) {

	ifstream iFile(filename.GetString(), ios::in);
	if (!iFile) {
		cerr << "Error in SubdObj::ReadWavefrontFile(), file " << filename
			 << "not found" << endl;
		return *this;
	}

	Destroy();

	VertexList VL;
	This_and_VL TaVL;

	TaVL.This = this;
	TaVL.VL = &VL;
	TaVL.face_count = 0;


	_hash_table_size = 475921;
	_loading_hash_table = new HalfEdgePtr[_hash_table_size];
	for (int i=0; i<_hash_table_size; i++)
		_loading_hash_table[i] = NULL;


	WF_Read_Lite(iFile, VertexCallback, FaceCallback, &TaVL, &TaVL);

	cerr << "SubdObj::ReadWavefrontFile(): read " << TaVL.face_count
		 << " faces" << endl;


	delete [] _loading_hash_table;
	_loading_hash_table = NULL;

	ClearChildren();


	VL.RemoveAllButDontDelete();

	return *this;
}


/*
#define TK_F 100
#define TK_V 101
#define TK_SLASH 102

SubdObj& SubdObj::ReadWavefrontFile(const JLString &filename) {

	int i;
	JLString s;
	Scanner *SC;
	int maxverts = 0;
	float x, y, z;

	Destroy();

	SC = new Scanner(filename);
	SC->AddToken("v", TK_V);
	SC->AddToken("f", TK_F);

	int done = 0;
	while (!done) {
		SC->GetNextToken();
		if (SC->GetID() == TK_V)
			maxverts++;
		else if (SC->GetID() == TK_F)
			done = 1;
	}

	delete SC;

	cout << "Reading Wavefront file with " << maxverts << " verteces." << endl;

	if (maxverts < 3) {
		cerr << "Error!  Must define at least 3 verticies!" << endl;
		exit(-1);
	}

	_hash_table_size = maxverts * 2 + 1;
	_loading_hash_table = new HalfEdgePtr[_hash_table_size];

	for (i=0; i<_hash_table_size; i++)
		_loading_hash_table[i] = NULL;

	VertexPtr *verts = new VertexPtr[maxverts];

	SC = new Scanner(filename);
	SC->AddToken("v", TK_V);
	SC->AddToken("f", TK_F);
	SC->AddToken("t", TK_SLASH);

	for (i=0; i<maxverts; i++) {

		while (SC->GetNextToken().GetID() != TK_V)
			if (SC->GetID() == TK_EOF) {
				cerr << "Error: only read " << i << " verteces" << endl;
				exit(-1);
			}

		if (SC->GetNextToken().GetID() != TK_FLOAT) {
			cerr << "Error: float value expected" << endl;
			exit(-1);
		}
		x = SC->GetToken().Val();

		if (SC->GetNextToken().GetID() != TK_FLOAT) {
			cerr << "Error: float value expected" << endl;
			exit(-1);
		}
		y = SC->GetToken().Val();

		if (SC->GetNextToken().GetID() != TK_FLOAT) {
			cerr << "Error: float value expected" << endl;
			exit(-1);
		}
		z = SC->GetToken().Val();

		verts[i] = NewVertex(Vec4(x, y, z));
	}


	VertexPtr vertlist[50];

	done = 0;
	int count_verts = 0;
	int count_faces = 0;
	int can_read = 0;

	while (!done) {

		SC->GetNextToken();

		switch (SC->GetID()) {

			case TK_EOF:
				done = 1;
				cerr << "Got EOF!" << endl;
				if (count_verts > 0) {
					AddFace(vertlist, count_verts);
					count_verts = 0;
					count_faces++;
					if ((count_faces % 100) == 0)
						cerr << "Read face " << count_faces << endl;
				}
				break;

			case TK_F:
				if (count_verts > 0) {
					AddFace(vertlist, count_verts);
					count_verts = 0;
					count_faces++;
					if ((count_faces % 100) == 0)
						cerr << "Read face " << count_faces << endl;
				}
				can_read = 1;
				break;

			case TK_FLOAT:
				if (can_read) {
					int v = (int) SC->GetToken().Val();
					if (v > maxverts) {
						cerr << "Error: vertex " << v << ", max: " << maxverts
							 << endl;
						exit(-1);
					}
					vertlist[count_verts++] = verts[v-1];
				}
				break;

			case TK_SLASH:
				SC->GetNextToken();
				break;

			default:
				if (count_verts > 0) {
					AddFace(vertlist, count_verts);
					count_verts = 0;
					count_faces++;
					if ((count_faces % 100) == 0)
						cerr << "Read face " << count_faces << endl;
				}
				can_read = 0;
				break;
		}
	}

	cout << "Loaded " << count_faces << " faces." << endl;

	delete [] verts;

	delete [] _loading_hash_table;
	_loading_hash_table = NULL;

	ClearChildren();

	return *this;
}
*/


Vertex* SubdObj::NewVertex(const Vec4 &v, int check_for_duplicate) {

	Vertex *vtx;

	if (check_for_duplicate) {
		for (vtx = _vertex_list; vtx; vtx = vtx->_next) {
			const Vec4 &vec = vtx->_point;
			if ((vec.x() == v.x()) &&
				(vec.y() == v.y()) &&
				(vec.z() == v.z())) {
//					cerr << "Vertex shared!" << endl;
					return vtx;
			}
		}
	}

//	cerr << "Vertex created!" << endl;

	vtx = new Vertex(v);

	if (_vertex_list)
		_v_tail = _v_tail->_next = vtx;
	else
		_vertex_list = _v_tail = vtx;

	return _v_tail;
}

/*
Vertex* SubdObj::NewVertex(const Vec4 &v) {

	Vertex *vtx = new Vertex(v);

	if (_vertex_list)
		_v_tail = _v_tail->_next = vtx;
	else
		_vertex_list = _v_tail = vtx;

	return _v_tail;
}
*/

HalfEdge* SubdObj::NewHalfEdgeHalf(Vertex *v1, HalfEdge *other) {

	HalfEdge *he = new HalfEdge(v1);
	he->_other_half = other;
	other->_other_half = he;

	if (_half_edge_list)
		_he_tail->_next = he;
	else
		_half_edge_list = he;

	_he_tail = he;

	return he;
}

HalfEdge* SubdObj::NewHalfEdgePair(Vertex *v1, Vertex *v2) {

	HalfEdge *he = new HalfEdge(v1);
	he->_other_half = new HalfEdge(v2);
	he->_other_half->_other_half = he;

	he->_next = he->_other_half;

	if (_half_edge_list)
		_he_tail->_next = he;
	else
		_half_edge_list = he;

	_he_tail = he->_other_half;

	if (_loading_hash_table) {
		int hash = Hash(v1->_flag, v2->_flag, _hash_table_size);
		he->_child = _loading_hash_table[hash];
		_loading_hash_table[hash] = he;
	}

	return he;
}

HalfEdge* SubdObj::FindHalfEdgePair(Vertex *v1, Vertex *v2) {

	int hash = Hash(v1->_flag, v2->_flag, _hash_table_size);

	for (HalfEdge *he = _loading_hash_table[hash]; he; he = he->_child) {
		if ((he->_v == v2) && (he->_other_half->_v == v1))
			return he->_other_half;
		if ((he->_v == v1) && (he->_other_half->_v == v2))
			return he;
	}

/*
	for (he = _half_edge_list; he; he = he->_next)
		if ((he->_v == v1) && (he->_other_half->_v == v2))
			return he;
*/

	return NewHalfEdgePair(v1, v2);
}

/*
HalfEdge* SubdObj::FindPreviousHalfEdge(HalfEdge *first) {

	HalfEdge *he = first;

	while (he->_link->_other_half != first)
		he = he->_link->_other_half;

	return he;
}
*/

SubdObj& SubdObj::ClearHalfEdgeFlags() {

	if (_half_edge_list)
		for (HalfEdge *he = _half_edge_list; he; he = he->_next)
			he->_flag = 0;

	return *this;
}

SubdObj& SubdObj::ClearVertexFlags() {

	if (_vertex_list)
		for (Vertex *v = _vertex_list; v; v = v->_next)
			v->_flag = 0;

	return *this;
}

SubdObj& SubdObj::ClearChildren() {

	if (_half_edge_list)
		for (HalfEdge *he = _half_edge_list; he; he = he->_next)
			he->_child = NULL;

	return *this;
}

SubdObj& SubdObj::LinkFace(HalfEdge *he1, HalfEdge *he2, HalfEdge *he3) {

	he1->_link = he2->_other_half;
	he2->_link = he3->_other_half;
	he3->_link = he1->_other_half;

	return *this;
}

SubdObj& SubdObj::LinkFace(HalfEdge *he1, HalfEdge *he2,
						   HalfEdge *he3, HalfEdge *he4) {

	he1->_link = he2->_other_half;
	he2->_link = he3->_other_half;
	he3->_link = he4->_other_half;
	he4->_link = he1->_other_half;

	return *this;
}

SubdObj& SubdObj::AddFace(VertexPtr *vertlist, int size) {

	if (size < 3) {
		cerr << "Error in AddFace: Need at least three verticies!" << endl;
		return *this;
	}

	HalfEdge *first = FindHalfEdgePair(vertlist[1], vertlist[0]);
	HalfEdge *next = FindHalfEdgePair(vertlist[2], vertlist[1]);
	HalfEdge *old = first;

	old->_link = next->_other_half;
	old = next;

	for (int v=3; v<size; v++) {
		next = FindHalfEdgePair(vertlist[v], vertlist[v-1]);
		old->_link = next->_other_half;
		old = next;
	}

	next = FindHalfEdgePair(vertlist[0], vertlist[size-1]);
	old->_link = next->_other_half;
	next->_link = first->_other_half;

	return *this;
}

int SubdObj::VerifyTriangleMesh() {

	for (HalfEdge *he = _half_edge_list; he; he=he->_next) {
		if ((!he->_other_half->_link) ||
			(!he->_other_half->_link->_other_half->_link) ||
			(he->_other_half->_link->_other_half->_link->_other_half->_link
			 															!= he))
				return 0;
	}

	return 1;
}

int SubdObj::CountTriangles() {

	ClearHalfEdgeFlags();

	int tris=0;

	for (HalfEdge *he = _half_edge_list; he; he=he->_next) {
		if (!he->_flag) {
			he->_flag = 1;
			if (he->_other_half->_link) {
				tris++;
				for (HalfEdge *clear = he->_other_half->_link; clear != he;
											clear = clear->_other_half->_link)
					clear->_flag = 1;
			}
		}
	}

	return tris;
}

/*
SubdObj& SubdObj::MakeT() {

	const real scale = 0.75;
	const Vec4 trans(0, 0, 0);
//	const real fudge35 = 0.5;
	const real fudge35 = 0.0;
	const real back = -2;

	Destroy();

	Vertex *v1 = NewVertex(Vec4(back, 1, -3) * scale + trans);
	Vertex *v2 = NewVertex(Vec4(0, 1, -3) * scale + trans);
	Vertex *v3 = NewVertex(Vec4(fudge35, 1, -1) * scale + trans);
	Vertex *v4 = NewVertex(Vec4(2, 1, -1) * scale + trans);
	Vertex *v5 = NewVertex(Vec4(fudge35, 1, 1) * scale + trans);
	Vertex *v6 = NewVertex(Vec4(2, 1, 1) * scale + trans);
	Vertex *v7 = NewVertex(Vec4(back, 1, 3) * scale + trans);
	Vertex *v8 = NewVertex(Vec4(0, 1, 3) * scale + trans);

	Vertex *v9 = NewVertex(Vec4(back, -1, -3) * scale + trans);
	Vertex *v10 = NewVertex(Vec4(0, -1, -3) * scale + trans);
	Vertex *v11 = NewVertex(Vec4(fudge35, -1, -1) * scale + trans);
	Vertex *v12 = NewVertex(Vec4(2, -1, -1) * scale + trans);
	Vertex *v13 = NewVertex(Vec4(fudge35, -1, 1) * scale + trans);
	Vertex *v14 = NewVertex(Vec4(2, -1, 1) * scale + trans);
	Vertex *v15 = NewVertex(Vec4(back, -1, 3) * scale + trans);
	Vertex *v16 = NewVertex(Vec4(0, -1, 3) * scale + trans);

	AddFace(v7, v1, v2, v3, v4, v6, v5, v8);
	AddFace(v9, v15, v16, v13, v14, v12, v11, v10);
	AddFace(v7, v8, v16, v15);
	AddFace(v8, v5, v13, v16);
	AddFace(v5, v6, v14, v13);
	AddFace(v6, v4, v12, v14);
	AddFace(v4, v3, v11, v12);
	AddFace(v3, v2, v10, v11);
	AddFace(v2, v1, v9, v10);
	AddFace(v1, v7, v15, v9);

	return *this;
}

SubdObj& SubdObj::MakeT2() {

	const real scale = 0.75;
	const Vec4 trans(0, 0, 0);
//	const real fudge35 = 0.5;
	const real fudge35 = 0.0;
	const real back = -2;

	Destroy();

	Vertex *v1 = NewVertex(Vec4(back, 1, -3) * scale + trans);
	Vertex *v2 = NewVertex(Vec4(0, 1, -3) * scale + trans);
	Vertex *v3 = NewVertex(Vec4(fudge35, 1, -1) * scale + trans);
	Vertex *v4 = NewVertex(Vec4(2, 1, -1) * scale + trans);
	Vertex *v5 = NewVertex(Vec4(fudge35, 1, 1) * scale + trans);
	Vertex *v6 = NewVertex(Vec4(2, 1, 1) * scale + trans);
	Vertex *v7 = NewVertex(Vec4(back, 1, 3) * scale + trans);
	Vertex *v8 = NewVertex(Vec4(0, 1, 3) * scale + trans);

	Vertex *v9 = NewVertex(Vec4(back, -1, -3) * scale + trans);
	Vertex *v10 = NewVertex(Vec4(0, -1, -3) * scale + trans);
	Vertex *v11 = NewVertex(Vec4(fudge35, -1, -1) * scale + trans);
	Vertex *v12 = NewVertex(Vec4(2, -1, -1) * scale + trans);
	Vertex *v13 = NewVertex(Vec4(fudge35, -1, 1) * scale + trans);
	Vertex *v14 = NewVertex(Vec4(2, -1, 1) * scale + trans);
	Vertex *v15 = NewVertex(Vec4(back, -1, 3) * scale + trans);
	Vertex *v16 = NewVertex(Vec4(0, -1, 3) * scale + trans);

	Vertex *v17 = NewVertex(Vec4(back, 1, -1) * scale + trans);
	Vertex *v18 = NewVertex(Vec4(back, 1, 1) * scale + trans);
	Vertex *v19 = NewVertex(Vec4(back, -1, -1) * scale + trans);
	Vertex *v20 = NewVertex(Vec4(back, -1, 1) * scale + trans);

	AddFace(v7, v18, v5, v8);
	AddFace(v17, v3, v5, v18);
	AddFace(v1, v2, v3, v17);

	AddFace(v3, v4, v6, v5);

	AddFace(v15, v16, v13, v20);
	AddFace(v19, v20, v13, v11);
	AddFace(v9, v19, v11, v10);

	AddFace(v11, v13, v14, v12);

	AddFace(v7, v8, v16, v15);
	AddFace(v8, v5, v13, v16);
	AddFace(v5, v6, v14, v13);
	AddFace(v6, v4, v12, v14);
	AddFace(v4, v3, v11, v12);
	AddFace(v3, v2, v10, v11);
	AddFace(v2, v1, v9, v10);

	AddFace(v1, v17, v19, v9);
	AddFace(v17, v18, v20, v19);
	AddFace(v18, v7, v15, v20);
//	AddFace(v1, v7, v15, v9);

	return *this;
}
*/

void SubdObj::CalcCenterAndRadius(Vec4 &C, float &r) {

	if (!_vertex_list)
		return;

	float min_max[2][3];
	min_max[0][0] = min_max[1][0] = _vertex_list->_point.x();
	min_max[0][1] = min_max[1][1] = _vertex_list->_point.y();
	min_max[0][2] = min_max[1][2] = _vertex_list->_point.z();

	for (Vertex *v = _vertex_list; v; v=v->_next)
		v->_point.AugmentBound(min_max);

	C.Set((min_max[0][0] + min_max[1][0]) * 0.5f,
		  (min_max[0][1] + min_max[1][1]) * 0.5f,
		  (min_max[0][2] + min_max[1][2]) * 0.5f);

	float dx = min_max[0][0] - min_max[1][0];
	float dy = min_max[0][1] - min_max[1][1];
	float dz = min_max[0][2] - min_max[1][2];

	r = fsqrt(dx*dx + dy*dy + dz*dz) * 0.5f;
}

SubdObj& SubdObj::JitterVertecies(real j) {

	for (Vertex *v = _vertex_list; v; v=v->_next)
		v->_point += Vec4(drand48()*j, drand48()*j, drand48()*j);

	return *this;
}

Vec4 SubdObj::CalcAveragePoint(HalfEdge *first) {

	int total = 1;
	Vec4 Sum = first->_v->Point();

	HalfEdge *he = first->_link->_other_half;

	while (he && (he != first)) {
		Sum += he->_v->Point();
		total++;
		he = he->_link->_other_half;
	}

	Sum /= total;
	return Sum;
}

SubdObj& SubdObj::CalcAveragePointWithNormal(HalfEdge *first, Vertex &v,
											 JLmatrix *VT, JLmatrix *IVT) {

	Vec4 vtmp, ntmp;

	first->_v->_point.FastJLmatrixMultiplyAndDivideByW(vtmp, *IVT);
	first->_v->_color_or_normal.FastJLmatrixMultiplyAndDivideByW(ntmp, *IVT);

	int total = 1;
	v._point = vtmp;
	v._color_or_normal = ntmp;

	HalfEdge *he = first->_link->_other_half;

	while (he && (he != first)) {
		he->_v->_point.FastJLmatrixMultiplyAndDivideByW(vtmp, *IVT);
		he->_v->_color_or_normal.FastJLmatrixMultiplyAndDivideByW(ntmp, *IVT);
		Vec4FastAdd(v._point, v._point, vtmp);
		Vec4FastAdd(v._color_or_normal, v._color_or_normal, ntmp);
		total++;
		he = he->_link->_other_half;
	}

	/* it may not be necessary to normalize normal here... */

	Vec4FastCopyScale(vtmp, v._point, 1.0f / float(total));
	Vec4FastCopyScale(ntmp, v._color_or_normal, 1.0f / float(total));

	Vec4FastSub(ntmp, ntmp, vtmp);
	ntmp.MakeUnit();
	Vec4FastAdd(ntmp, ntmp, vtmp);

	vtmp.FastJLmatrixMultiplyAndDivideByW(v._point, *VT);
	ntmp.FastJLmatrixMultiplyAndDivideByW(v._color_or_normal, *VT);

	return *this;
}

SubdObj& SubdObj::CalcAveragePointWithNormal(HalfEdge *first, Vertex &v) {

	int total = 1;
	v._point = first->_v->_point;
	v._color_or_normal = first->_v->_color_or_normal;

	HalfEdge *he = first->_link->_other_half;

	while (he && (he != first)) {
		Vec4FastAdd(v._point, v._point, he->_v->_point);
		Vec4FastAdd(v._color_or_normal, v._color_or_normal,
													he->_v->_color_or_normal);
		total++;
		he = he->_link->_other_half;
	}

	/* it may not be necessary to normalize normal here... */

	v._point.Vec4FastScale3(1.0f / float(total));
	v._color_or_normal.Vec4FastScale3(1.0f / float(total));

	Vec4FastSub(v._color_or_normal, v._color_or_normal, v._point);
	v._color_or_normal.MakeUnit();
	Vec4FastAdd(v._color_or_normal, v._color_or_normal, v._point);

	return *this;
}

Vec4 SubdObj::CalcAverageWithNextPoint(HalfEdge *first) {

	static Vec4 Ave;

	Vec4FastAverage(Ave, first->_v->Point(),
						 first->_link->_other_half->_v->Point());

	return Ave;
}

Vec4 SubdObj::CalcAverageWithPrevPoint(HalfEdge *first) {

	static Vec4 Ave;

	Vec4FastAverage(Ave, first->_v->Point(),
						 first->_other_half->_v->Point());

	return Ave;
}

Vec4 SubdObj::CalcFaceNormal(HalfEdge *first) {

	HalfEdge *second = first->_link->_other_half;

	Vec4 v1 = second->_v->Point() - second->_link->_other_half->_v->Point();
	Vec4 v2 = first->_v->Point() - second->_v->Point();

	Vec4 Tmp = v1.Cross3(v2);
	Tmp.MakeUnit();
	return Tmp;
}

void SubdObj::CalcVertexNormal(HalfEdge *first) {

	// Vec4 Sum = first->_other_half->_v->Point();
	Vec4 Sum(0,0,0);
	// Vec4 Tmp;

	HalfEdge *he = first;

/*
	do {
		Tmp = first->_v->Point() - he->_other_half->_v->Point();
		Tmp.MakeUnit();
		Sum += Tmp;
		he = he->_link;
	} while (he != first);
*/

	do {
		if (he->_link)
			Sum += CalcFaceNormal(he);
		he = he->_link;
	} while (he && he != first);

	Sum.MakeUnit();
	first->_v->SetNormal(Sum + first->_v->Point());
}

SubdObj& SubdObj::CalcAllVertexNormals() {

	for (HalfEdge *he = _half_edge_list; he; he = he->_next)
		if (!(he->_v->_has_normal))
			CalcVertexNormal(he);

	return *this;
}

SubdObj& SubdObj::ReverseAllVertexNormals() {

	for (Vertex *v = _vertex_list; v; v = v->_next)
		if (v->_has_normal)
			Vec3_FastWeightedSum(v->GetNormal(),
								v->GetNormal(), -1, v->GetPoint(), 2);

	return *this;
}

int SubdObj::CountSides(HalfEdge *first) {

	// This has never been tested:

	int count = 1;

	HalfEdge *he = first->_next;

	while (he != first) {
		count++;
		he = he->_link->_other_half;
	}

	return count;
}

SubdObj& SubdObj::DooSabin(int parts) {

	HalfEdge *he;
	SubdObj *doo = new SubdObj;

	if (!_half_edge_list)
		return *this;

	ClearChildren();

	for (he = _half_edge_list; he; he = he->_next)
		if (!he->_child && he->_link)
			DooSabinFace(he, doo);

	if (!parts || (parts > 1))
		for (he = _half_edge_list; he; he = he->_next)
			if (he->_child && he->_other_half->_child &&
				!(he->_child->_other_half->_link))
					DooSabinEdge(he, doo);

	if (!parts || (parts > 2))
		for (he = _half_edge_list; he; he = he->_next)
		  if (he->_child && he->_other_half->_child && he->_child->_link->_link)
			DooSabinVertex(he);

	Destroy();

	_half_edge_list = doo->_half_edge_list;
	_he_tail = doo->_he_tail;
	_vertex_list = doo->_vertex_list;
	_v_tail = doo->_v_tail;

	doo->_half_edge_list = doo->_he_tail = NULL;
	doo->_vertex_list = doo->_v_tail = NULL;

	delete doo;

	return *this;
}

SubdObj& SubdObj::DooSabinFace(HalfEdge *first, SubdObj *doo) {

	HalfEdge *he = first, *last = NULL;
	Vertex *v;

	Vec4 middle = CalcAveragePoint(first);
	Vec4 tmp, tmp2;
	Vec4 PrevMid, NextMid;

	NextMid = CalcAverageWithPrevPoint(first);

	do {

		PrevMid = NextMid;
		NextMid = CalcAverageWithNextPoint(he);

// Calc average of four:

//		Vec4FastAverage(tmp, he->_v->_point, middle);
		Vec3_FastWeightedSum(tmp, he->_v->_point, _not_midpoint,
								 middle, 1-_not_midpoint);
		Vec4FastAverage(tmp2, PrevMid, NextMid);
		Vec4FastAverage(tmp, tmp, tmp2);

		v = doo->NewVertex(tmp, 0);

		if (last) {
			he->_child = doo->NewHalfEdgePair(v, last->_child->_v);
			last->_child->_link = he->_child->_other_half;
		}
		else
			he->_child = doo->NewHalfEdgePair(v, NULL);

		last = he;
		he = he->_link->_other_half;

	} while (he != first);

	last->_child->_link = first->_child->_other_half;
	first->_child->_other_half->_v = v;

	return *this;
}

SubdObj& SubdObj::DooSabinEdge(HalfEdge *root, SubdObj *doo) {

	HalfEdge *top = doo->NewHalfEdgePair(
				root->_child->_v, root->_other_half->_child->_other_half->_v);
	HalfEdge *bottom = doo->NewHalfEdgePair(
				root->_other_half->_child->_v, root->_child->_other_half->_v);

	top->_link = root->_child;
	bottom->_link = root->_other_half->_child;

	root->_child->_other_half->_link = bottom->_other_half;
	root->_other_half->_child->_other_half->_link = top->_other_half;

	return *this;
}

SubdObj& SubdObj::DooSabinVertex(HalfEdge *root) {

	root->_child->_link->_link->_link = root->_other_half->_child->_other_half->_link->_other_half;

	return *this;
}

SubdObj& SubdObj::CatmullClark() {

	HalfEdge *stophere = _he_tail;
	HalfEdge *he;

	ClearVertexFlags();
	ClearHalfEdgeFlags();

/*
	for (he = _half_edge_list; he != stophere; he = he->_next)
		cout << he << endl;
	cout << endl;
*/

	for (he = _half_edge_list; he != stophere; he = he->_next)
		if (!he->_child)
			CatmullClarkSplitEdge(he);
//	if (!stophere->_child)
//		CatmullClarkSplitEdge(stophere);

	for (he = _half_edge_list; he != stophere; he = he->_next)
		CatmullClarkLinkChildren(he);
	CatmullClarkLinkChildren(stophere);

	for (he = _half_edge_list; he != stophere; he = he->_next)
		if (!he->_child->_v)
			CatmullClarkFaceVertex(he->_child);
//	if (!stophere->_child->_v)
//		CatmullClarkFaceVertex(stophere->_child);

	for (he = _half_edge_list; he != stophere; he = he->_next)
		if (he->_child)
			CatmullClarkVertexVertex(he);
//	if (stophere->_child)
//		CatmullClarkVertexVertex(stophere);

	for (he = _half_edge_list; he != stophere; he = he->_next)
		if (he->_other_half->_v->_flag)
			CatmullClarkEdgeVertex(he->_other_half);
//	if (stophere->_other_half->_v->_flag)
//		CatmullClarkEdgeVertex(stophere->_other_half);

	return *this;
}

SubdObj& SubdObj::CatmullClarkSplitEdge(HalfEdge *bottom) {

	HalfEdge *top = bottom->_other_half;

	Vec4 Average;
	Vec4FastAverage(Average, bottom->_v->Point(), top->_v->Point());
	Vertex *v = NewVertex(Average, 0);
	v->_flag = 1;	// flag means we still need to recalculate edge point

	HalfEdge *other_top = NewHalfEdgeHalf(v, top);
	HalfEdge *other_bottom = NewHalfEdgeHalf(v, bottom);

	HalfEdge *left = NewHalfEdgePair(v, NULL);
	HalfEdge *right = NewHalfEdgePair(v, NULL);

	other_bottom->_link = right;
	right->_link = other_top;
	other_top->_link = left;
	left->_link = other_bottom;

	bottom->_child = right->_other_half;
	top->_child = left->_other_half;

	return *this;
}

SubdObj& SubdObj::CatmullClarkLinkChildren(HalfEdge *he) {

	he->_child->_other_half->_link->_other_half->_link->_child->_link =
		he->_child;

	return *this;
}

SubdObj& SubdObj::CatmullClarkFaceVertex(HalfEdge *first) {

	HalfEdge *he = first;
	Vec4 Sum;
	int total = 0;

	do {
		Sum += he->_other_half->_link->_other_half->_v->Point();
		total++;
		he = he->_link;
	} while (he != first);

	Sum /= total;
	Vertex *v = NewVertex(Sum, 0);

	do {
		he->_v = v;
		he = he->_link;
	} while (he != first);

	return *this;
}

SubdObj& SubdObj::CatmullClarkVertexVertex(HalfEdge *first) {

	Vec4 Q, R;
	Vec4 S = first->_v->Point();

	HalfEdge *he = first;
	int total = 0;

	do {
		Q += he->_child->_v->Point();
		R += he->_other_half->_v->Point();
		total++;
		he->_child = NULL;
		he = he->_link;
	} while (he != first);

	Q /= (total * total);
	R *= (2.0 / (real) (total * total));
	S *= (real) (total - 3) / total;

	Vec4FastAdd(Q, Q, R);
	Vec4FastAdd(first->_v->Point(), Q, S);

	first->_v->_has_normal = 0;

	return *this;
}

SubdObj& SubdObj::CatmullClarkEdgeVertex(HalfEdge *he) {

	HalfEdge *he2 = he->_link->_link;
	Vertex *v = he->_v;

	v->_flag = 0;

	v->Point() *= 2;
	v->Point() += he->_link->_other_half->_v->Point();
	v->Point() += he2->_link->_other_half->_v->Point();

	v->Point() /= 4;

	return *this;
}


void SubdObj::CalcEdgeFrame(const HalfEdge *he,
							Vec4 &Center,
							Vec4 &Forward, Vec4 &Right, Vec4 &Up) {

	Vec4FastAverage(Center, he->_v->Point(), he->_other_half->_v->Point());

	Vec4FastSub(Forward, he->_v->Point(), he->_other_half->_v->Point());

	float scale = Forward.Length();
	Forward /= scale;


	Vec4 Tmp, LeftPlane, RightPlane;

	Vec4FastSub(Tmp, he->_link->_other_half->_v->Point(),
					 he->_v->Point());

	Vec4FastCross3(LeftPlane, Forward, Tmp);
	LeftPlane.MakeUnit();

	Vec4FastSub(Tmp, he->_other_half->_v->Point(),
					 he->_other_half->_link->_other_half->_v->Point());

	Vec4FastCross3(RightPlane, Forward, Tmp);
	RightPlane.MakeUnit();

	Vec4FastAverage(Up, LeftPlane, RightPlane);
	Up.MakeUnit();

//	Up = LeftPlane;


	Vec4FastCross3(Right, Forward, Up);


	scale *= 0.25;

	Forward *= scale;
	Right *= scale;
	Up *= scale;
}