////////////////////////////////////////////////////////////////////////////////
// Mesh.cpp
// Copyright Bjoern Ganster
// Developed since 2001
////////////////////////////////////////////////////////////////////////////////

#include "Mesh.h"
#include "Matrix.h"
#include "MemoryBlock.h"
#include "BGCPString.h"

#include <iostream>

#define FILE_NUMBER FILE_NO_MESH_CPP

//#define SHOW_OPEN_EDGES

////////////////////////////////////////////////////////////////////////////////
// Constructor

Mesh::Mesh (const char* className, BGClassID classID, RenderableObject* parent)
: RenderableObject(className, classID, parent),
  mWireframeColor(0.0f, 0.0f, 1.0f),
  mTexture(NULL),
  points(),
  polygons(),
  mShadowProxy(nullptr)
{
	SET_SCOPE;
	mDefaultPolygonColor = Color (1.0, 1.0, 1.0);
	RETURN;
}

////////////////////////////////////////////////////////////////////////////////
// Destructor

Mesh::~Mesh ()
{
	SET_SCOPE;
	clear();
	safeDelete(mShadowProxy);
	RETURN;
}

////////////////////////////////////////////////////////////////////////////////
// Add a point

size_t Mesh::addPoint(double x, double y, double z)
{
	SET_SCOPE;
	size_t base = points.size();
	points.push_back(x);
	points.push_back(y);
	points.push_back(z);
	RETURN base/3;
}

////////////////////////////////////////////////////////////////////////////////

bool Mesh::getVertex(size_t index, Point3D& p) const
{
	SET_SCOPE;
	size_t base = 3 * index;
	if (base + 2 < points.size()) {
		p.x = points[base];
		p.y = points[++base];
		p.z = points[++base];
		p.w = 1.0;
		RETURN true; 
	} else {
		RETURN false;
	}
}

void Mesh::setVertex(size_t index, const Point3D& p) 
{
	SET_SCOPE;
	size_t base = 3*index;
	points[base] = p.x;
	points[++base] = p.y;
	points[++base] = p.z;
	RETURN;
}

////////////////////////////////////////////////////////////////////////////////

void Mesh::glVertex(size_t index) const
{
	size_t base = 3 * index;
	if (base + 2 < points.size()) {
		glVertex3d(points[base], points[base+1], points[base+2]);
	} else {
		glVertex3d(0.0, 0.0, 0.0);
	}
}

////////////////////////////////////////////////////////////////////////////////
// Render a polygon set

void Mesh::renderSelf()
{
	SET_SCOPE;
	if ((getFlags() & RenderableObject::Flag_invisible) != 0) {
		RETURN;
	}

	bool textured;
	if (mTexture != nullptr && mTexture->getWidth() > 0) {
		glEnable (GL_TEXTURE_2D);
		//glEnableClientState (GL_TEXTURE_COORD_ARRAY);
		mTexture->useMipmap();
		//mTexture->use();
		textured = true;
	} else {
		glDisable (GL_TEXTURE_2D);
		textured = false;
	}

	color.glColor();
	//std::vector<float> triangleVertices, triangleTexCoords, triangleNormals, quadVertices, quadTexCoords, quadNormals;

	for (size_t i = 0; i < polygons.size(); ++i) {
		SET_OP;
		MeshPolygon& poly = *polygons[i];
		/*if (poly.getPointCount() == 3) {
			for (size_t j = 0; j < 3; ++j) {
				auto ji = poly.getVertexIndex(j);
				Point3D p = getTransformedVertex(ji);
				double u, v;
				auto ti = poly.getTexCoordIndex(j);
				getTexCoords(ti, u, v);
				triangleVertices.push_back((float) p.x);
				triangleVertices.push_back((float) p.y);
				triangleVertices.push_back((float) p.z);
				triangleTexCoords.push_back((float) u);
				triangleTexCoords.push_back((float) v);
				triangleNormals.push_back((float) poly.normal.x);
				triangleNormals.push_back((float) poly.normal.y);
				triangleNormals.push_back((float) poly.normal.z);
			}
		} else if (poly.getPointCount() == 4) {
			for (size_t j = 0; j < 4; ++j) {
				auto ji = poly.getVertexIndex(j);
				Point3D p = getTransformedVertex(ji);
				double u, v;
				auto ti = poly.getTexCoordIndex(j);
				getTexCoords(ti, u, v);
				quadVertices.push_back((float)p.x);
				quadVertices.push_back((float)p.y);
				quadVertices.push_back((float)p.z);
				quadTexCoords.push_back((float)u);
				quadTexCoords.push_back((float)v);
				//Point3D n = poly.getNormal(j);
				quadNormals.push_back((float)poly.normal.x);
				quadNormals.push_back((float)poly.normal.y);
				quadNormals.push_back((float)poly.normal.z);
			}
		} else {*/
			// Currently faster ...
			poly.render(textured, true);
		//}
	}

	/*glEnableClientState (GL_VERTEX_ARRAY);
	glEnableClientState(GL_NORMAL_ARRAY);

	glVertexPointer (3, GL_FLOAT, 0, triangleVertices.data());
	glTexCoordPointer (2, GL_FLOAT, 0, triangleTexCoords.data());
	glNormalPointer(GL_FLOAT, 0, triangleNormals.data());
	glDrawArrays(GL_TRIANGLES, 0, triangleVertices.size()/3);

	glVertexPointer (3, GL_FLOAT, 0, quadVertices.data());
	glTexCoordPointer (2, GL_FLOAT, 0, quadTexCoords.data());
	glNormalPointer(GL_FLOAT, 0, quadNormals.data());
	glDrawArrays(GL_QUADS, 0, quadVertices.size()/3);

	glDisableClientState (GL_VERTEX_ARRAY);
	glDisableClientState(GL_NORMAL_ARRAY);

	if (textured) {
		glDisable (GL_TEXTURE_2D);
		glDisableClientState (GL_TEXTURE_COORD_ARRAY);
	}*/

	// Highlight open edges so they can be fixed ...
	#ifdef SHOW_OPEN_EDGES
	if (mMeshEdges.size() == 0) {
		findMeshEdges();
	}

	// Show open edges in red
	glDisable(GL_DEPTH_TEST);
	glDisable(GL_LIGHTING);
	glDisable(GL_TEXTURE_2D);
	glColor3d(1.0, 0.0, 0.0);
	glBegin(GL_LINES);
		for (size_t i = 0; i < mMeshEdges.size(); ++i) {
			MeshEdge& edge = mMeshEdges[i];
			if (edge.polies.size() % 2 != 0) {
				Point3D p = getTransformedVertex(edge.vertexIndex1);
				Point3D q = getTransformedVertex(edge.vertexIndex2);
				p.glVertex();
				q.glVertex();
			}
		}
	glEnd();

	// If the edge count of reversed and unreversed edges is not equal, show those edges in green
	glColor3d(0.0, 1.0, 0.0);
	glBegin(GL_LINES);
	for (size_t i = 0; i < mMeshEdges.size(); ++i) {
		MeshEdge& edge = mMeshEdges[i];
		int reversed = 0;
		int nonReversed = 0;
		for (size_t j = 0; j < edge.edgeReversedInPoly.size(); ++j) {
			if (edge.edgeReversedInPoly[j])
				++reversed;
			else
				++nonReversed;
		}
		
		if (edge.polies.size() > 1 
		&&  reversed != nonReversed) {
			Point3D p = getTransformedVertex(edge.vertexIndex1);
			Point3D q = getTransformedVertex(edge.vertexIndex2);
			p.glVertex();
			q.glVertex();
		}
	}
	glEnd();

	// Finally, show n-gons in blue ...
	glColor3d(0.5, 0.5, 1.0);
	glBegin(GL_LINES);
	for(size_t i = 0; i < polygons.size(); ++i) {
		MeshPolygon* poly = polygons[i];
		if (poly != nullptr && poly->getVertexCount() > 4) {
			for (size_t j = 0; j < poly->getVertexCount(); ++j) {
				auto k1 = poly->getVertexIndex(j);
				getTransformedVertex(k1).glVertex();
				auto k2 = poly->getVertexIndex((j+1) % poly->getVertexCount());
				getTransformedVertex(k2).glVertex();
			}
		}
	}
	glEnd();
	glEnable(GL_DEPTH_TEST);
	glEnable(GL_LIGHTING);
	#endif

	RETURN;
}

void Mesh::renderWireframe()
{
	SET_SCOPE;

	if ((getFlags() & RenderableObject::Flag_invisible) != 0) {
		RETURN;
	}

	glPushMatrix();
	mM.glMultMatrix();

	for (size_t i = 0; i < mChilds.size(); ++i) {
		RenderableObject* child = mChilds[i];
		child->renderWireframe();
	}

	mWireframeColor.glColor();
	glDepthFunc(GL_LEQUAL);

	/*glBegin(GL_LINES);
	for (size_t i = 0; i < polygons.size(); ++i) {
		MeshPolygon* poly = polygons[i];
		size_t polyVertices = poly->getPointCount();
		for (size_t j = 0; j < polyVertices; ++j) {
			Point3D p1 = poly->getVertexCoords(j);
			p1.glVertex();
			Point3D p2 = poly->getVertexCoords((j+1) % polyVertices);
			p2.glVertex();
		}
	}
	glEnd();*/

	const double LINE_WIDTH = 0.1;
	glEnable(GL_POLYGON_OFFSET_FILL);
	glPolygonOffset(0.0, -1.0);
	glBegin(GL_QUADS);
	for (size_t i = 0; i < polygons.size(); ++i) {
		SET_OP;
		MeshPolygon* poly = polygons[i];
		size_t polyVertices = poly->getVertexCount();
		Point3D mid;
		for (size_t j = 0; j < polyVertices; ++j) {
			mid += poly->getVertexCoords(j);
		}
		mid = mid / polyVertices;
		for (size_t j = 0; j < polyVertices; ++j) {
			size_t k = (j+1) % polyVertices;
			Point3D pj = poly->getVertexCoords(j);
			Point3D pk = poly->getVertexCoords(k);
			Point3D qj = (1.0 - LINE_WIDTH) * pj + LINE_WIDTH * mid;
			Point3D qk = (1.0 - LINE_WIDTH) * pk + LINE_WIDTH * mid;

			pj.glVertex();
			qj.glVertex();
			qk.glVertex();
			pk.glVertex();
		}
	}
	glEnd();
	glDisable(GL_POLYGON_OFFSET_FILL);

	glPopMatrix();
	RETURN;
}

////////////////////////////////////////////////////////////////////////////////
// Get min/max mesh coords, taking into account transformations

bool Mesh::getMinCoordsHere(Point3D& value) 
{
	// We cannot update min/max here because we do not know the parent transforms. Instead, once very frame, cacheTransformedVertices
	// should be called from the scene root
	SET_SCOPE;
	value = mMinCoords;
	RETURN (points.size() != 0);
}

bool Mesh::getMaxCoordsHere(Point3D& value) 
{
	// We cannot update min/max here because we do not know the parent transforms. Instead, once very frame, cacheTransformedVertices
	// should be called from the scene root
	SET_SCOPE;
	value = mMaxCoords;
	RETURN (points.size() != 0);
}

////////////////////////////////////////////////////////////////////////////////
// Mark front-facing polygons

void Mesh::markFrontFacingPolygons(const Point3D& ls)
{
	SET_SCOPE;
	if (mShadowProxy != nullptr) {
		mShadowProxy->markFrontFacingPolygons(ls);
		RETURN;
	}

	if (polygons.size() > 0 && mMeshEdges.size() == 0) {
		findMeshEdges();
	}

	for (size_t i = 0; i < polygons.size(); i++) {
		MeshPolygon& poly = *polygons[i];
		auto p0i = poly.getVertexIndex(0);
		Point3D p0 = getTransformedVertex(p0i);
		poly.mark = poly.transformedNormal * (ls - p0);
	}

	for (size_t i = 0; i < getObjectCount(); ++i) {
		RenderableObject* ro = getObject(i);
		if (ro != nullptr) {
			ro->markFrontFacingPolygons(ls);
		}
	}
	RETURN;
}

////////////////////////////////////////////////////////////////////////////////
// Render shadow volume

void Mesh::renderShadowVolumes (const Point3D& ls) const
{
	SET_SCOPE;

	for (size_t i = 0; i < getObjectCount(); ++i) {
		const RenderableObject* ro = getObject(i);
		if (ro != nullptr) {
			ro->renderShadowVolumes(ls);
		}
	}

	if (mShadowProxy != nullptr) {
		mShadowProxy->renderShadowVolumes(ls);
		RETURN;
	}

	if ((getFlags() & RenderableObject::Flag_waterTight) == 0) {
		RETURN;
	}

	SET_OP;
	for (size_t i = 0; i < mMeshEdges.size(); i++) {
		const MeshEdge& edge = mMeshEdges[i];
		bool reverseEdge = false;

		// A shadow polygon is created for this edge if either there is no
		// opposed edge or the edge and the opposed edge differ in lighting
		bool RenderShadowEdge = false;
		if (edge.polies.size() < 2) {
			// shouldn't get here under the watertight condition...
			RenderShadowEdge = false;
		} else {
			SET_OP;
			bool frontFacingPolies = false;
			bool backFacingPolies = false;
			SET_OP;
			for (size_t j = 0; j < edge.polies.size(); ++j) {
				SET_OP; // Crashes here on 2023-04-10 ...
				if (edge.polies[j] != nullptr) {
					SET_OP;
					if (edge.polies[j]->mark < 0) {
						SET_OP;
						frontFacingPolies = true;
					} else { //if (edge.polies[j]->mark > 0) { // case mark==0 has to be treated as well ...
						SET_OP;
						backFacingPolies = true;
						reverseEdge = !edge.edgeReversedInPoly[j]; // ???negate???
					}
				}
			}
			SET_OP;
			RenderShadowEdge = (frontFacingPolies && backFacingPolies);
		}

		if (RenderShadowEdge) {
			SET_OP;
			// p1, q1 represent the edge
			Point3D p1 = getTransformedVertex(edge.vertexIndex1);
			Point3D q1 = getTransformedVertex(edge.vertexIndex2);
			Point3D p2 = -ls; // todo: assumes that ls has w=0

			// Render shadow polygon
			// todo: for light sources not at infinity, issue quads here ...
			glBegin(GL_TRIANGLES);
				if (reverseEdge) {
					p2.glVertex();
					q1.glVertex ();
					p1.glVertex ();
				} else {
					p1.glVertex();
					q1.glVertex();
					p2.glVertex();
				}
			glEnd();
		}
	}

	/*glBegin(GL_TRIANGLES);
	for (size_t i = 0; i < shadowTriangleCoords.size(); i += 3) {
		glVertex3d(shadowTriangleCoords[i], shadowTriangleCoords[i+1], shadowTriangleCoords[i+2]);
	}
	glEnd();*/

	/*glEnableClientState (GL_VERTEX_ARRAY);
	glEnableClientState(GL_NORMAL_ARRAY);
	glVertexPointer (3, GL_FLOAT, 0, shadowTriangleCoords.data());
	glDrawArrays(GL_TRIANGLES, 0, shadowTriangleCoords.size()/3);
	glDisableClientState (GL_VERTEX_ARRAY);
	glDisableClientState(GL_NORMAL_ARRAY);*/

	// Use the polygons facing the light source to close the shadow volume
	SET_OP;
	for (size_t i = 0; i < polygons.size(); ++i) {
		const MeshPolygon* poly = polygons[i];
		if (poly->mark > 0) { // ???
			poly->render(false, true);
		}
	}

	RETURN;
}

///////////////////////////////////////////////////////////////////////////////
// Load a mesh from a .obj

bool Mesh::loadFromObj (const PathChar* fileName)
{
	SET_SCOPE;
	MemoryBlock mb;
	if (!mb.loadFromFile(fileName)) {
		BGCPString msg = BGCPString("Failed to open ") + BGCPString(fileName, CP_SYSTEM);
		msg.printLn();
		RETURN false;
	}

	// Read file
	size_t offset = 0;
	size_t EOL = 0;
	size_t nextLineOffset = 0;
	mb.findEOL<char>(offset, EOL, nextLineOffset);

	// Index 0 in obj refers to the origin
	addPoint(0.0, 0.0, 0.0);
	addTexCoords(0.0, 0.0);

	bool keepReading = true;
	while (keepReading) {
		BGCPString line;
		line.appendPart(mb.getPtr<char>(offset), EOL-offset, CP_ASCII);
		if (line.getByteCount() > 2) {
			char c1 = line.get<char>(0);
			char c2 = line.get<char>(1);
			if (c1 == 'v' && c2 == ' ') {
					// Read three coordinates from the file
					auto words = line.split(" ", false);
					if (words.size() > 3) {
						BGCPString& xStr = words[1];
						BGCPString& yStr = words[2];
						BGCPString& zStr = words[3];
						double x = xStr.toDouble();
						double y = yStr.toDouble();
						double z = zStr.toDouble();
						addPoint (x, y, z);
					}
			} else if (c1 == 'f' && c2 == ' ') 
			{
				// Count words
				size_t offset = 2;
				auto words = line.split(" ", false);
				size_t pointCount = words.size()-1;

				// Set polygon points
				MeshPolygon* newPoly = addPolygon(pointCount);
				size_t word = 0;
				size_t pnum = 0;

				while (pnum < pointCount && word < pointCount) {
					BGCPString& vertexData = words[word+1];
					size_t splitPos = vertexData.scan("/", 0, vertexData.getByteCount());
					BGCPString vnum = vertexData.copyPart(0, splitPos);
					BGCPString tnum = vertexData.copyPart(splitPos+1, vertexData.getByteCount());
					auto newV = vnum.toInt();
					word++;

					// Check if vertex number is legal
					if (newV <= getVertexCount() && newV != 0) {
						auto t = tnum.toInt();
						newPoly->setVertexIndex((size_t)pnum, (size_t)newV);
						newPoly->setTexCoordIndex((size_t)pnum, (size_t)t);
						pnum++;
					}
				}
			} else if (c1 == 'v' && c2 == 't') {
					auto words = line.split(" ", false);
					if (words.size() > 2) {
						BGCPString& ustr = words[1];
						BGCPString& vstr = words[2];
						double u = ustr.toDouble();
						double v = vstr.toDouble();
						textureCoords.push_back(u);
						textureCoords.push_back(v);
					}
			}
		}

		offset = nextLineOffset;
		mb.findEOL<char>(offset, EOL, nextLineOffset);
		keepReading = (offset < mb.getSize());
	}

	calculateNormals();

	RETURN true;
}

///////////////////////////////////////////////////////////////////////////////
// Calculate normals for polygons and vertices

void Mesh::calculateNormals(double scaleFactor)
{
	SET_SCOPE;
	// Calculate normals for polygons
	for (size_t i = 0; i < polygons.size(); ++i) {
		MeshPolygon* poly = polygons[i];
		if (poly->getVertexCount() > 2) {
			auto p0i = poly->getVertexIndex(0);
			auto p1i = poly->getVertexIndex(1);
			auto p2i = poly->getVertexIndex(2);
			Point3D p0 = getTransformedVertex(p0i);
			Point3D p1 = getTransformedVertex(p1i);
			Point3D p2 = getTransformedVertex(p2i);
			Point3D d1 = p0-p1;
			Point3D d2 = p0-p2;
			d1.unitLength();
			d2.unitLength();
			poly->transformedNormal = scaleFactor * d1.crossMultiply (d2);
			poly->transformedNormal.unitLength();
		}
	}

	// Calculate normals for the vertices from surrounding triangles
	/* for (i = 0; i < points.getSize (); i++) {
	Point3D& p = *points[i];
	p.normal = Point3D (0.0, 0.0, 0.0);
	for (j = 0; j < p.triangles.size (); j++) {
	p.normal = p.normal + p.triangles[j]->normal;
	}

	if (p.normal.length () == 0.0)
	p.normal = Point3D (1.0, 0.0, 0.0);
	else
	p.normal.UnitLength ();
	}*/

	RETURN;
}

///////////////////////////////////////////////////////////////////////////////
// Save to obj

bool Mesh::saveToObj (const PathChar* FileName, bool saveUVs)
{
	SET_SCOPE;
	BGCPString lines;
	if (!mName.empty()) {
		lines = BGCPString("g ") + getName() + BGCPString("\n");
	}
	size_t uvBaseForPoly = 1;

	// Write vertices
	for (size_t i = 0; i < points.size(); i += 3) {
		BGCPString line = BGCPString("v ") + BGCPString::fromDouble(points[i]) + BGCPString(" ") + 
			BGCPString::fromDouble(points[i+1]) + BGCPString(" ") +
			BGCPString::fromDouble(points[i+2]) + BGCPString("\n");
		lines += line;
	}

	for (size_t i = 0; i < polygons.size(); i++) {
		MeshPolygon& poly = *polygons[i];

		// Write UVs - optional
		if (saveUVs) { // && poly.hasTextureCoords()) {
			for (size_t j = 0; j < poly.getVertexCount(); j++) {
				double u, v;
				auto ti = poly.getTexCoordIndex(j);
				getTexCoords(ti, u, v);
				BGCPString line = BGCPString("vt ") + BGCPString::fromDouble (u) + BGCPString(" ") + 
					BGCPString::fromDouble(v) + BGCPString("\n");
				lines += line;
			}
		}

		// Write faces
		BGCPString line("f");
		for (size_t j = 0; j < poly.getVertexCount(); j++) {
			line += BGCPString(" ");
			auto vertexNum = poly.getVertexIndex(j);
			line += BGCPString::fromInt(vertexNum+1);
			if (saveUVs) {
				line += BGCPString("/");
				auto texCoordNum = poly.getTexCoordIndex(j);
				line += BGCPString::fromInt(uvBaseForPoly + j);
			}
		}
		line += BGCPString("\n");
		lines += line;
		uvBaseForPoly += poly.getVertexCount();
	}

	lines.saveToFile(FileName, CP_ASCII);
	RETURN true;
}

///////////////////////////////////////////////////////////////////////////////
// Reduce polygons to triangles

// Not all algorithms can cope with polygons consisting of more than 3 vertices
// (that is, non-triangles). Therefore, this method reduces such polygons to 
// triangles. This is done by connecting all edges to the first vertex of the 
// polygon for polygons consisting of four points. For other polygons, a new
// point is created in the middle of the polygon and connected to the edges of 
// the polygon.

void Mesh::reducePolygonsToTriangles ()
{
/*   unsigned int i = 0;
   while (i < polygons.size ()) {
      MeshPolygon* poly = polygons[i];
      if (poly->getEdgeCount () == 4) {
         addTriangle (poly->getPoint (0)->num,
                      poly->getPoint (1)->num,
                      poly->getPoint (2)->num);
         addTriangle (poly->getPoint (0)->num,
                      poly->getPoint (2)->num,
                      poly->getPoint (3)->num);

         // Delete old triangle and remove it from the vector
         removePolygon (poly);
      } else if (poly->getEdgeCount () > 4) {
         // Compute center of polygon
         unsigned int j;
         Point3D center = *poly->getPoint (0);
         size_t polysize = poly->getEdgeCount ();
         for (j = 1; j < polysize; j++) {
            center = center + *poly->getPoint (j);
         }
         center = center / polysize;
         Point3D* centerpoint = addPoint (center.x, center.y, center.z);
         size_t centernum = centerpoint->num;

         // Create new triangles from polygon
         for (j = 0; j < polysize; j++) {
            unsigned int k = (j+1) % polysize;
            addTriangle (centernum, 
                         poly->getPoint (j)->num,
                         poly->getPoint (k)->num);
         }

         // Remove old triangle
         removePolygon (poly);
      } else {
         // Process next polygon
         i++;
      }
   }
   calculateNormals ();*/
}

///////////////////////////////////////////////////////////////////////////////
// Set per vertex lighting

void Mesh::setPerVertexLighting (bool newVal)
{
	SET_SCOPE;
	for (unsigned int i = 0; i < polygons.size(); i++) {
		polygons[i]->PerVertexLighting = newVal;
	}
	RETURN;
}

////////////////////////////////////////////////////////////////////////////////
// Recursive Part of mesh size calculation

void Mesh::getSizeRecursive (Point3D& minCoords, Point3D& maxCoords, 
	size_t& vertices, size_t& polygons, const Matrix& m) const
{
	SET_SCOPE;
	Matrix m2 = m * getMatrix();
	if (vertices == 0 && points.size() > 0) {
		Point3D p;
		getVertex(0, p);
		p = m2.Transform(p);
		maxCoords = p;
		minCoords = p;
	}

	for (size_t i = 0; i < getVertexCount(); i++) {
		Point3D p;
		getVertex(i,p);
		p = m2.Transform(p);
		minCoords = MinCoords(minCoords, p);
		maxCoords = MaxCoords(maxCoords, p);
	}
	polygons += this->getPolygonCount();
	vertices += this->getVertexCount();
	RenderableObject::getSizeRecursive(minCoords, maxCoords, vertices, polygons, m);
	RETURN;
}

// Transform all vertices using m
void Mesh::applyMatrix(const Matrix& m)
{
	SET_SCOPE;
	Matrix m2 = mM * m;

	// Update vertices stored directly in this object
	for (size_t i = 0; i < getVertexCount(); ++i) {
		Point3D p;
		getVertex(i, p);
		Point3D q = m2.Transform(p);
		setVertex(i, q);
	}

	if (mShadowProxy != nullptr) {
		mShadowProxy->applyMatrix(m2); // Doesn't work?
	}

	RenderableObject::applyMatrix(m2);

	// The previous local matrix was applied also, so it has to be cleared now
	mM = Matrix();
	RETURN;
}

// Set texture coordinates for a vertex
bool Mesh::setTexCoords (size_t i, double u, double v)
{
	SET_SCOPE;
	i *= 2;
	if (i > 0 && i+1 < textureCoords.size()) {
		textureCoords[i] = u;
		textureCoords[i+1] = v;
		RETURN true;
	} else {
		RETURN false;
	}
}

// Add new texture coordinates to this mesh and return their index
size_t Mesh::addTexCoords (double u, double v)
{
	SET_SCOPE;
	size_t index = textureCoords.size() / 2;
	textureCoords.push_back(u);
	textureCoords.push_back(v);
	RETURN index;
}

// Get texture coordinates
bool Mesh::getTexCoords (size_t i, double& u, double& v) const
{
	SET_SCOPE;
	i *= 2;
	if (i+1 < textureCoords.size()) {
		u = textureCoords[i];
		v = textureCoords[i+1];
		RETURN true;
	} else {
		u = 0.0;
		v = 0.0;
		RETURN false;
	}
}

void Mesh::setVertexCoords(size_t polyBaseIndex, size_t indexInPoly, double x, double y, double z)
{
	SET_SCOPE;
	size_t index = 3*(polyBaseIndex+indexInPoly);
	if (index+2 < points.size()) {
		points[index]   = x;
		points[index+1] = y;
		points[index+2] = z;
	}
	RETURN;
}

// Get number of texture coordinates
size_t Mesh::getTexCoordCount() const
{
	return textureCoords.size() / 2;
}

// Get vertex coordinates
/*Point3D Mesh::getVertexCoords(size_t i) const 
{
	SET_SCOPE;
	size_t index = 3*i;
	if (index+2 < points.size()) {
		RETURN Point3D(points[index], points[index+1], points[index+2]);
	} else {
		RETURN Point3D();
	}
}*/

MeshPolygon* Mesh::addPolygon(size_t vertexCount)
{
	SET_SCOPE;
	size_t oldSize = points.size();
	size_t newIndex = oldSize/3;
	MeshPolygon* newPoly = new MeshPolygon(vertexCount, this, newIndex);
	polygons.push_back(newPoly);
	RETURN newPoly;
}

// Prepare to render polygon
/*void Mesh::setRenderingBuffers(size_t mIndexInMesh, size_t vertexCount, bool textured)
{
    glVertexPointer(3, GL_DOUBLE, 0, &points[3*mIndexInMesh]);
    if (textured) {
        glTexCoordPointer(2, GL_DOUBLE, 0, &textureCoords[2*mIndexInMesh]);
    }
    glDrawArrays (GL_POLYGON, 0, vertexCount);
}*/

// Copy data from other mesh (not including childs)
void Mesh::copyMesh(const Mesh* other)
{
	if (other == nullptr) {
		return;
	}

	SET_SCOPE;
	mTexture = other->mTexture;
	mDefaultPolygonColor = other->mDefaultPolygonColor;
	points = other->points;
	textureCoords = other->textureCoords;
	mMinCoords = other->mMinCoords;
	mMaxCoords = other->mMaxCoords;
	setName(other->getName());

	for (size_t i = 0; i < other->polygons.size(); ++i) {
		SET_OP;
		MeshPolygon* otherPoly = other->polygons[i];
		MeshPolygon* newPoly = MeshPolygon::copy(otherPoly, this);
		polygons.push_back(newPoly);
	}

	RETURN;
}

// Transform all vertices in the scene and cache them
void Mesh::cacheTransformedVertices(const Matrix& m)
{
	SET_SCOPE;

	if (mShadowProxy != nullptr) {
		mShadowProxy->cacheTransformedVertices(m);
	}

	auto vertexCount = getVertexCount();
	mTransformedVertices.resize(vertexCount);

	// Check childs
	RenderableObject::cacheTransformedVertices(m);

	if (vertexCount > 0) {
		SET_OP;
		// Cache transformed vertices
		for (size_t j = 0; j < vertexCount; ++j) {
			SET_OP;
			Point3D p;
			getVertex(j, p);
			mTransformedVertices[j] = m.Transform(p);
		}

		SET_OP;
		mMinCoords = mTransformedVertices[0];
		mMaxCoords = mTransformedVertices[0];

		for (size_t i = 1; i < vertexCount; ++i) {
			SET_OP;
			min_max_update(mMinCoords.x, mMaxCoords.x, mTransformedVertices[i].x);
			min_max_update(mMinCoords.y, mMaxCoords.y, mTransformedVertices[i].y);
			min_max_update(mMinCoords.z, mMaxCoords.z, mTransformedVertices[i].z);
		}

		// Recalculate normals from transformed vertices
		// Works. Not sure if this is even slower than the matrix operations ... introduces about 10% performance loss 
		// compared to not doing this, but without it, normals do not adjust ...
		SET_OP;
		calculateNormals();
	}

	RETURN;
}

Point3D Mesh::getTransformedVertex(size_t index) const
{
	// Try to return transformed coordinates
	if (index < mTransformedVertices.size()) {
		return mTransformedVertices[index];
	} else {
		// If we don't have transformed coordinates, return untransformed coordinates (if available)
		Point3D p;
		getVertex(index, p);
		return p;
	}
}

void Mesh::glTransformedVertex(size_t index) const
{
	// Try to return transformed coordinates
	if (index < mTransformedVertices.size()) {
		mTransformedVertices[index].glVertex();
	} else {
		// If we don't have transformed coordinates, return untransformed coordinates (if available)
		glVertex(index);
	}
}

// Make sure all childs use the same texture
void Mesh::applyTexture(Texture* aTexture)
{
	mTexture = aTexture;

	for (size_t i = 0; i < getObjectCount(); ++i) {
		RenderableObject* ro = getObject(i);
		if (ro != nullptr) {
			ro->applyTexture(aTexture);
		}
	}
}

// Polygon access
size_t Mesh::getPolygonCount() const
{
	return polygons.size();
}

MeshPolygon* Mesh::getPolygon(size_t num)
{
	SET_SCOPE;
	if (num < polygons.size()) {
		RETURN polygons[num];
	} else {
		RETURN nullptr;
	}
}

const MeshPolygon* Mesh::getPolygon(size_t num) const
{
	SET_SCOPE;
	if (num < polygons.size()) {
		RETURN polygons[num];
	} else {
		RETURN nullptr;
	}
}

// Set wireframe color for this object and its childs
void Mesh::setRecursiveWireframeColor(const Color& c)
{
	SET_SCOPE;
	mWireframeColor = c;
	for (size_t i = 0; i < mChilds.size(); ++i) {
		mChilds[i]->setRecursiveWireframeColor(c);
	}
	RETURN;
}

size_t Mesh::addPoint(const Point3D& p)
{
	return addPoint (p.x, p.y, p.z);
}

// Delete all children (for meshes, also clears the mesh)
void Mesh::clear()
{
	SET_SCOPE;
	for (size_t i = 0; i < polygons.size(); i++) {
		safeDelete(polygons[i]);
	}

	SET_OP;
	polygons.clear();
	points.clear();
	textureCoords.clear();
	mMeshEdges.clear();
	mTransformedVertices.clear();
	RenderableObject::clear();

	RETURN;
}

// Get point statistics
size_t Mesh::getRecursivePointCount() const 
{
	return points.size();
}
size_t Mesh::getVertexCount() const
{
	return points.size() / 3;
}

/*void Mesh::scaleNormals(double factor)
{
	for (size_t i = 0; i < polygons.size(); ++i) {
		MeshPolygon* poly = polygons[i];
		poly->normal = poly->normal * factor;
	}
}*/

// Find polygon edges
void Mesh::findPolygonEdges(MeshPolygon* poly)
{
	SET_SCOPE;
	size_t badEdges = 0;

	for (size_t j = 0; j < poly->getVertexCount(); ++j) {
		unsigned int v1 = poly->getVertexIndex(j);
		unsigned int v2 = poly->getVertexIndex((j + 1) % poly->getVertexCount());
		MeshEdge* edgeMatch = nullptr;
		bool reversedInEdge = false;
		Point3D p1 = getTransformedVertex(v1);
		Point3D p2 = getTransformedVertex(v2);
		double tolerance = 0.005 * (p2 - p1).length(); // magic constant ...

		// Find best matching edge
		for (size_t k = 0; k < mMeshEdges.size(); ++k) {
			MeshEdge& edge = mMeshEdges[k];
			Point3D ep1 = getTransformedVertex(edge.vertexIndex1);
			Point3D ep2 = getTransformedVertex(edge.vertexIndex2);
			double newDist1 = (p1 - ep1).length() + (p2 - ep2).length();
			double newDist2 = (p1 - ep2).length() + (p2 - ep1).length();

			if (newDist1 < tolerance) {
				edgeMatch = &edge;
				reversedInEdge = false;
				tolerance = newDist1;
			}

			if (newDist2 < tolerance) {
				edgeMatch = &edge;
				reversedInEdge = true;
				tolerance = newDist2;
			}
		}

		if (edgeMatch != nullptr) {
			// Found a matching edge
			edgeMatch->polies.push_back(poly);
			edgeMatch->edgeReversedInPoly.push_back(reversedInEdge);
		} else {
			if (v1 != v2) {
				// Create new edge
				MeshEdge newEdge;
				newEdge.vertexIndex1 = v1;
				newEdge.vertexIndex2 = v2;
				newEdge.polies.push_back(poly);
				newEdge.edgeReversedInPoly.push_back(false);
				mMeshEdges.push_back(newEdge);
			} else {
				++badEdges;
			}
		}
	}

	if (badEdges > 0) {
		//COUT << badEdges << " bad edges" << std::endl;
	}

	RETURN;
}

// Find mesh edges
void Mesh::findMeshEdges()
{
	SET_SCOPE;
	mMeshEdges.clear();

	// Add all edges in all polygons to mMeshEdges
	for (size_t i = 0; i < polygons.size(); ++i) {
		MeshPolygon* poly = polygons[i];
		findPolygonEdges(poly);
	}

	// Check if this is a watertight mesh
	bool isWaterTight = true;
	for (size_t i = 0; i < mMeshEdges.size(); ++i) {
		MeshEdge& edge = mMeshEdges[i];
		if (edge.polies.size() % 2 != 0) {
			isWaterTight = false;
		}
	}

	if (isWaterTight) {
		setFlag(RenderableObject::Flag_waterTight);
		RETURN;
	} else {
		//clearFlag(RenderableObject::Flag_waterTight);
		if ((getFlags() & RenderableObject::Flag_waterTight) != 0) {
			RETURN;
		}
	}

	if (mShadowProxy != nullptr) {
		mShadowProxy->findMeshEdges();
		RETURN;
	}
	RETURN;
}

// Create triangles from pairs of edges that share a vertex in order to fill mesh holes
// Not fully tested yet. May change the convex hull of an object, so use with caution.
bool Mesh::fillHoles()
{
	SET_SCOPE;
	size_t failedToCloseEdges = 0;
	// In order to make meshes watertight, we need to do the following:
	// 1. Find an open edge
	// 2. Find another open edge that shares a corner with the first edge
	// 3. Create a new triangle from these two edges (calculate a normal and assign default texture coordinates)
	//    (While the normal isn't important, the orientation is important. We can choose the orientation based on neighbouring polygons or facing
	//     away from the mesh center, which would have to be computed in advance)
	// 4. Add the new edges to the edge list
	// 5. If no open edges remain, we are done
	// 6. If we have added less than the allowed threshold, continue from step 1

	int trianglesAdded = 0;
	for (size_t i = 0; i+1 < mMeshEdges.size(); ++i) {
		MeshEdge& edge1 = mMeshEdges[i];
		Point3D p1 = getTransformedVertex(edge1.vertexIndex1);
		Point3D p2 = getTransformedVertex(edge1.vertexIndex2);
		if (edge1.polies.size() < 2) {
			size_t j = i+1;
			bool closedEdge = false;
			while (j < mMeshEdges.size()) {
				MeshEdge& edge2 = mMeshEdges[j];
				if (edge2.polies.size() < 2) {
					// Check if one of the edge vertices in edge2 equals one of the edge vertices in edge1
					Point3D q1 = getTransformedVertex(edge2.vertexIndex1);
					Point3D q2 = getTransformedVertex(edge2.vertexIndex2);
					bool match = false;
					size_t v3index = 0;
					double maxVertexDist = 0.01;

					// If one vertex of the edge is very near the other edge, use the other vertex of the edge to create the new triangle
					// todo: instead of greedily grabbing the first edge that fits the criteria, choose the best edge out of all edges
					double d1 = (p1-q1).length();
					if (d1 < maxVertexDist) {
						v3index = edge2.vertexIndex2;
						match = true;
						maxVertexDist = d1;
					}

					double d2 = (p2-q1).length();
					if (d2 < maxVertexDist) {
						v3index = edge2.vertexIndex2;
						match = true;
						maxVertexDist = d2;
					}

					double d3 = (p1-q2).length();
					if (d3 < maxVertexDist) {
						v3index = edge2.vertexIndex1;
						match = true;
						maxVertexDist = d3;
					}

					double d4 = (p2-q2).length();
					if (d4 < maxVertexDist) {
						v3index = edge2.vertexIndex1;
						match = true;
						maxVertexDist = d4;
					}

					if (match) {
						// Create a new triangle from the edges
						// The new triangle is set up to have different orientation than the edge that first defined it
						MeshPolygon* newTriangle = addTriangle();
						newTriangle->setVertexIndex(0, edge1.vertexIndex2);
						newTriangle->setVertexIndex(1, edge1.vertexIndex1);
						newTriangle->setVertexIndex(2, v3index);
						closedEdge = true;

						// todo: triangle normal and texture coords ...
						//edge1.polies.push_back(newTriangle);
						//edge2.polies.push_back(newTriangle);

						// Create new edge or update existing one... todo
						/*MeshEdge newEdge;
						newEdge.vertexIndex1 = v1;
						newEdge.vertexIndex2 = v2;
						newEdge.polies.push_back(poly);
						newEdge.edgeReversedInPoly.push_back(false);
						mMeshEdges.push_back(newEdge);*/

						// Update edge lists ...
						findPolygonEdges(newTriangle);

						++trianglesAdded;
						j = mMeshEdges.size();

						if (trianglesAdded > 500) {
							COUT << "Cannot make watertight: ";
							getName().printLn();
							RETURN false;
						}
					}
				}
				++j;

				if (!closedEdge) {
					++failedToCloseEdges;
				}
			}
		}
	}

	if (failedToCloseEdges > 0) {
		#ifdef _DEBUG
		COUT << "Failed to close " << failedToCloseEdges << " edges on ";
		getName().printLn();
		#endif
		RETURN true;
	} else {
		//COUT << "Made watertight: ";
		//getName().printLn();
		setFlag(RenderableObject::Flag_waterTight);
		RETURN true;
	}
}

// Close a mesh by creating a new triangle for each open edge that connects to a point in the middle of the mesh
bool Mesh::fillHoles2()
{
	SET_SCOPE;
	if (getVertexCount() < 3) {
		RETURN true;
	}

	Point3D middle;
	for (size_t i = 0; i < getVertexCount(); ++i) {
		middle += getTransformedVertex(i);
	}
	middle = middle / getVertexCount();
	auto newVertexIndex = addPoint(middle);

	int trianglesAdded = 0;
	for (size_t i = 0; i < mMeshEdges.size(); ++i) {
		MeshEdge& edge1 = mMeshEdges[i];
		Point3D p1 = getTransformedVertex(edge1.vertexIndex1);
		Point3D p2 = getTransformedVertex(edge1.vertexIndex2);
		if (edge1.polies.size() < 2
		&&  edge1.vertexIndex1 != newVertexIndex
		&&  edge1.vertexIndex2 != newVertexIndex) 
		{
			// Create a new triangle from the edges
			// The new triangle is set up to have different orientation than the edge that first defined it
			MeshPolygon* newTriangle = addTriangle();
			newTriangle->setVertexIndex(0, edge1.vertexIndex2);
			newTriangle->setVertexIndex(1, edge1.vertexIndex1);
			newTriangle->setVertexIndex(2, newVertexIndex);

			// todo: triangle normal and texture coords ...

			// Update edge lists ...
			// todo: directly add newTriangle to edge1, edge2, but edge3 may need to be created as in findPolygonEdges
			// Refactor findPolygonEdges to handle edges instead of whole polygons?
			findPolygonEdges(newTriangle);
			++trianglesAdded;

			if (trianglesAdded > 500) {
				COUT << "Cannot make watertight: ";
				getName().printLn();
				RETURN false;
			}
		}
	}

	RETURN true;
}

// Close a mesh by creating a new triangle for each open edge that connects to point given as an argument
bool Mesh::fillHoles3(const Point3D& newP)
{
	SET_SCOPE;
	if (getVertexCount() < 3) {
		RETURN true;
	}

	auto newVertexIndex = addPoint(newP);

	int trianglesAdded = 0;
	for (size_t i = 0; i < mMeshEdges.size(); ++i) {
		MeshEdge& edge1 = mMeshEdges[i];
		Point3D p1 = getTransformedVertex(edge1.vertexIndex1);
		Point3D p2 = getTransformedVertex(edge1.vertexIndex2);
		if (edge1.polies.size() < 2
			&&  edge1.vertexIndex1 != newVertexIndex
			&&  edge1.vertexIndex2 != newVertexIndex) 
		{
			// Create a new triangle from the edges
			// The new triangle is set up to have different orientation than the edge that first defined it
			MeshPolygon* newTriangle = addTriangle();
			newTriangle->setVertexIndex(0, edge1.vertexIndex2);
			newTriangle->setVertexIndex(1, edge1.vertexIndex1);
			newTriangle->setVertexIndex(2, newVertexIndex);

			// todo: triangle normal and texture coords ...

			// Update edge lists ...
			// todo: directly add newTriangle to edge1, edge2, but edge3 may need to be created as in findPolygonEdges
			// Refactor findPolygonEdges to handle edges instead of whole polygons?
			findPolygonEdges(newTriangle);
			++trianglesAdded;

			if (trianglesAdded > 500) {
				COUT << "Cannot make watertight: ";
				getName().printLn();
				RETURN false;
			}
		}
	}

	RETURN true;
}

// Copy shadow proxy from other Mesh
void Mesh::copyShadowProxy(Mesh* other)
{
	safeDelete(mShadowProxy);
	mShadowProxy = new Mesh();
	mShadowProxy->copyMesh(other);
}

MeshPolygon* Mesh::addTriangle (size_t vi1, size_t vi2, size_t vi3)
{
	if (vi1 >= getVertexCount() && vi2 >= getVertexCount() && vi3 >= getVertexCount()) {
		return nullptr;
	}

	MeshPolygon* poly = addPolygon(3);

	if (poly == nullptr) {
		return nullptr;
	}

	poly->setVertexIndex(0, vi1);
	poly->setVertexIndex(1, vi2);
	poly->setVertexIndex(2, vi3);
	return poly;
}

// Texture access
void Mesh::setTexture(Texture* newText)
{
	mTexture = newText;
}

const Texture*Mesh::getTexture() const
{
	return mTexture;
}

Texture* Mesh::getTexture()
{
	return mTexture;
}
void Mesh::mirror(double sx, double sy, double sz, double tolerance)
{
	std::vector<size_t> mirroredIndexes;

	// Choose vertices to mirror (vertices that remain the same are reused)
	size_t oldVertexCount= getVertexCount();
	for (size_t i = 0; i < oldVertexCount; ++i) {
		Point3D p;
		getVertex(i, p);
		Point3D q(sx*p.x, sy*p.y, sz*p.z);

		if ((p-q).length() > tolerance) {
			auto newIndex = addPoint(q);
			mirroredIndexes.push_back(newIndex);
		} else {
			mirroredIndexes.push_back(i);
		}
	}

	// Create new triangles
	size_t oldPolyCount = polygons.size();
	for (size_t i = 0; i < oldPolyCount; ++i) {
		MeshPolygon* oldPoly = polygons[i];
		size_t oldPolyVertexCount = oldPoly->getVertexCount();
		MeshPolygon* newPoly = addPolygon(oldPolyVertexCount);

		for (size_t j = 0; j < oldPolyVertexCount; ++j) {
			size_t oldIndex = oldPoly->getVertexIndex(j);
			newPoly->setVertexIndex(oldPolyVertexCount-j-1, mirroredIndexes[oldIndex]);
		}
	}
}

