/* Copyright 2004, 2005 Nicholas Bishop
 * 
 * This file is part of SharpConstruct.
 *
 * SharpConstruct is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * SharpConstruct is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with SharpConstruct; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA */

#include "Brush.h"
#include "Mesh.h"
#include "MeshHistory.h"
#include "Polygon3D.h"
#include "Utilities.h"
#include <cmath>
#include <fstream>
#include <queue>
#include <sstream>
#include <string>

#include <iostream>

using namespace SharpConstruct;
using Optimized::Normal3D;
using Optimized::Point3D;
using Optimized::Point3DVector;

Normal3D Mesh::_view_up_normal( 0, 0, 0 );
Normal3D Mesh::_view_right_normal( 0, 0, 0 );
Normal3D Mesh::_view_z_normal( 0, 0, 0 );
float Mesh::_selection_radius = 0;
Point3D Mesh::_current_selection( 0, 0, 0 );
Mesh::ModifiedGroup Mesh::_modified_vertex_indices;
Mesh::DamagedGroup Mesh::_damaged_triangles;
Mesh::DamagedGroup Mesh::_damaged_quads;
Point3D Mesh::_center( 0, 0, 0 );
float Mesh::_diameter( 1 );
unsigned Mesh::_selected_vertex[ 8 ];
Point3DVector Mesh::_selection_tube;
Point3DVector Mesh::_projected_locations;

Mesh::Mesh( Optimized::Point3DVector& locs,
	    Optimized::Point3DVector& norms,
	    std::vector< Color >& cols,
	    Optimized::Point3DVector& tnorms,
	    Optimized::Point3DVector& qnorms,
	    std::vector< Triangle >& tris,
	    std::vector< Quad >& quads,
	    std::vector< PolygonUsers >& users,
	    bool& saved,
	    VisibleElementData& visel,
	    std::string& filename )
: _filename( filename ),
  _saved( saved ),
  _vertex_locations( locs ),
  _vertex_normals( norms ),
  _vertex_colors( cols ),
  _triangles( tris ),
  _quads( quads ),
  _triangle_normals( tnorms ),
  _quad_normals( qnorms ),
  _vertex_users( users ),
  _visible_elements( visel )
{}

/** Saves any supported mesh type.
 *
 * Parses the filename string to find the extension, then calls the appropriate
 * function to do the actual saving.
**/
void Mesh::Save( std::string filename )
{
	std::string extension = filename.substr( filename.rfind( '.',
	                                         std::string::npos ) + 1 );
	
	transform( extension.begin(), extension.end(), extension.begin(), toupper );

	RestoreOrderData& ro = _visible_elements.RestoreOrder;
	if( 0 )//if( ro.Enabled && ro.Vertices.size() && ro.Triangles.size() && ro.Quads.size() )
	{
		Optimized::Point3DVector locs;
		std::vector< Color > cols;
		std::vector< Triangle > tris;
		std::vector< Quad > quads;

		// Reorder vertices
		for( unsigned i = 0; i < _vertex_locations.size(); ++i )
		{
			locs.push_back( _vertex_locations[ ro.Vertices[ i ] ] );
			cols.push_back( _vertex_colors[ ro.Vertices[ i ] ] );
		}
		_vertex_locations = locs;
		_vertex_colors = cols;

		// Reorder triangles
		for( unsigned i = 0; i < _triangles.size(); ++i )
		{
			tris.push_back( _triangles[ ro.Triangles[ i ] ] );
			for( unsigned j = 0; j < 3; ++j )
				tris.back().V[ j ] = ro.Vertices[ tris.back().V[ j ] ];
		}
		_triangles = tris;

		// Reorder quads
		for( unsigned i = 0; i < _quads.size(); ++i )
		{
			quads.push_back( _quads[ ro.Quads[ i ] ] );
			for( unsigned j = 0; j < 4; ++j )
				quads.back().V[ j ] = ro.Vertices[ quads.back().V[ j ] ];
		}
		_quads = quads;

		_recalculate_vertex_users();
		RecalculateAllNormals();
	}

	if( extension == "OBJ" )
		_save_obj( filename );
	else if( extension == "WRL" )
		_save_wrl( filename );
	else
		_save_obj( filename );
	
	_filename = filename;
	_saved = true;
	_visible_elements.RestoreOrder.Enabled = true;
}

void Mesh::HideBox( Optimized::Point3DVector& hb )
{
	Optimized::Point3DVector vis, inv;
	std::vector< Color > colvis, colinv;
	std::map< unsigned, unsigned > mpvis, mpinv;
	std::vector< Triangle > vistris, invtris;
	std::vector< Quad > visquads, invquads;

	RestoreOrderData& ro = _visible_elements.RestoreOrder;

	Optimized::Point3DVector n;
	n.resize( 4 );
	n[ 0 ].CalculatePlaneNormal( hb[ 0 ].Data(), hb[ 1 ].Data(), hb[ 4 ].Data() );
	n[ 1 ].CalculatePlaneNormal( hb[ 1 ].Data(), hb[ 2 ].Data(), hb[ 5 ].Data() );
	n[ 2 ].CalculatePlaneNormal( hb[ 2 ].Data(), hb[ 3 ].Data(), hb[ 5 ].Data() );
	n[ 3 ].CalculatePlaneNormal( hb[ 3 ].Data(), hb[ 0 ].Data(), hb[ 4 ].Data() );

	float d[ 4 ];
	for( unsigned i = 0; i <= 3; ++i )
		d[ i ] = hb[ i ].X() * n[ i ].X() + hb[ i ].Y() * n[ i ].Y() + hb[ i ].Z() * n[ i ].Z();

	for( unsigned i = 0; i < _vertex_locations.size(); ++i )
	{
		// Test vertex's location with each of the four planes
		bool inside = true;
		for( unsigned j = 0; j <= 3; ++j )
		{
			if( _vertex_locations[ i ].X() * n[ j ].X() +
			    _vertex_locations[ i ].Y() * n[ j ].Y() +
			    _vertex_locations[ i ].Z() * n[ j ].Z() < d[ j ] )
			{
				inside = false;
				break;
			}
		}

		// If it's inside OR is already hidden, add to the hidden map
		if( inside || i >= _visible_elements.Vertices )
		{
			mpinv[ i ] = inv.size();
			inv.push_back( _vertex_locations[ i ] );
			colinv.push_back( _vertex_colors[ i ] );
		}
		else // Add to the visible map
		{
			mpvis[ i ] = vis.size();
			vis.push_back( _vertex_locations[ i ] );
			colvis.push_back( _vertex_colors[ i ] );
		}
	}

	_visible_elements.Vertices = vis.size();

	// Copy the vertices back, visible first, then hidden
	_vertex_locations = vis;
	for( unsigned i = 0; i < inv.size(); ++i )
		_vertex_locations.push_back( inv[ i ] );

	// Same for colors
	_vertex_colors = colvis;
	_vertex_colors.insert( _vertex_colors.end(), colinv.begin(), colinv.end() );

	for( unsigned i = 0; i < _triangles.size(); ++i )
	{
		bool visible = true;
		for( unsigned j = 0; j <= 2; ++j )
		{
			if( mpinv.find( _triangles[ i ].V[ j ] ) != mpinv.end() )
			{
				visible = false;
				_triangles[ i ].V[ j ] = mpinv[ _triangles[ i ].V[ j ] ] + vis.size();
			}
			else
				_triangles[ i ].V[ j ] = mpvis[ _triangles[ i ].V[ j ] ];

		}
		if( visible )
			vistris.push_back( _triangles[ i ] );
		else
			invtris.push_back( _triangles[ i ] );
	}
	_visible_elements.Triangles = vistris.size();
	_triangles = vistris;
	_triangles.insert( _triangles.end(), invtris.begin(), invtris.end() );

	for( unsigned i = 0; i < _quads.size(); ++i )
	{
		bool visible = true;
		for( unsigned j = 0; j <= 3; ++j )
		{
			if( mpinv.find( _quads[ i ].V[ j ] ) != mpinv.end() )
			{
				visible = false;
				_quads[ i ].V[ j ] = mpinv[ _quads[ i ].V[ j ] ] + vis.size();
			}
			else
				_quads[ i ].V[ j ] = mpvis[ _quads[ i ].V[ j ] ];

		}
		if( visible )
			visquads.push_back( _quads[ i ] );
		else
			invquads.push_back( _quads[ i ] );
	}
	_visible_elements.Quads = visquads.size();
	_quads = visquads;
	_quads.insert( _quads.end(), invquads.begin(), invquads.end() );

	_recalculate_vertex_users();
	RecalculateAllNormals();

	ro.Vertices = mpvis;
	for( std::map< unsigned, unsigned >::iterator i = mpinv.begin();
	     i != mpinv.end(); ++i )
		mpinv[ i->first ] = i->second + vis.size();

	_change_operation( true, true );
}

void Mesh::_change_operation( const bool upol, const bool all )
{
	_saved = false;
	MeshHistory::Instance().Changed()( upol, all );
}

void Mesh::begin_creation_()
{
	_filename = "";
	Clear();
}

void Mesh::finalize_creation_( const bool modifiable )
{
	_vertex_normals.resize( _vertex_locations.size() );
	_vertex_colors.resize( _vertex_locations.size() );
	_triangle_normals.resize( _triangles.size() );
	_quad_normals.resize( _quads.size() );
	_reset_visibles();
	_recalculate_vertex_users();
	if( modifiable )
	{
		Unify();
		Flood( Color( 1, 1, 1 ), 1 );
	}
	RecalculateAllNormals();
	_visible_elements.RestoreOrder.Reset();
	CalculateBoundingSphere();
	_change_operation( true, true );
	_saved = true;
}

void Mesh::Clear()
{
	_vertex_locations.clear();
	_vertex_normals.clear();
	_vertex_colors.clear();
	_clear_polys();
}

const Optimized::Point3DVector& Mesh::VertexLocations() const
{
	return _vertex_locations;
}
Optimized::Point3DVector& Mesh::VertexLocations()
{
	return _vertex_locations;
}

const Optimized::Point3DVector& Mesh::VertexNormals() const
{
	return _vertex_normals;
}
Optimized::Point3DVector& Mesh::VertexNormals()
{
	return _vertex_normals;
}

const std::vector< Color >& Mesh::VertexColors() const
{
	return _vertex_colors;
}
std::vector< Color >& Mesh::VertexColors()
{
	return _vertex_colors;
}

/*Optimized::Point3DVector& Mesh::TriangleNormals()
{
	return _triangle_normals;
}
Optimized::Point3DVector& Mesh::QuadNormals()
{
	return _quad_normals;
}*/
const std::vector< Triangle >& Mesh::Triangles() const
{
	return _triangles;
}
std::vector< Triangle >& Mesh::Triangles()
{
	return _triangles;
}

const std::vector< Quad >& Mesh::Quads() const
{
	return _quads;
}
std::vector< Quad >& Mesh::Quads()
{
	return _quads;
}
std::vector< PolygonUsers >& Mesh::VertexUsers()
{
	return _vertex_users;
}

const Optimized::Point3DVector& Mesh::ProjectedLocations() const
{
	return _projected_locations;
}
Optimized::Point3DVector& Mesh::ProjectedLocations()
{
	return _projected_locations;
}

Optimized::Point3D& Mesh::CurrentSelection()
{
	return _current_selection;
}

void Mesh::SetSelectionRadius( const float r )
{
	_selection_radius = r;
}
float Mesh::SelectionRadius()
{
	return _selection_radius;
}

Mesh::DamagedGroup& Mesh::DamagedTriangles()
{
	return _damaged_triangles;
}
Mesh::DamagedGroup& Mesh::DamagedQuads()
{
	return _damaged_quads;
}

Optimized::Normal3D& Mesh::UpNormal()
{
	return _view_up_normal;
}
Optimized::Normal3D& Mesh::RightNormal()
{
	return _view_right_normal;
}
Optimized::Normal3D& Mesh::ZNormal()
{
	return _view_z_normal;
}

const VisibleElementData& Mesh::VisibleElements() const
{
	return _visible_elements;
}
VisibleElementData& Mesh::VisibleElements()
{
	return _visible_elements;
}

Mesh::ModifiedGroup& Mesh::ModifiedVertexIndices()
{
	return _modified_vertex_indices;
}

std::string Mesh::FileName() const
{
	return _filename;
}
void Mesh::SetFileName( const std::string& f )
{
	_filename = f;
}

/** Returns mesh data information.
 *
 * Returns a string in this format:
 * "[number of vertices] vert[ex/ices], [number of polygons] polygon[s]"
**/
std::string Mesh::GetModelStatisticsString() const
{
	std::ostringstream oss;

	oss << _vertex_locations.size();
	if( _vertex_locations.size() == 1 )
		oss << " vertex, ";
	else
		oss << " vertices, ";
	oss << _poly_count();
	if( _poly_count() == 1 )
		oss << " polygon";
	else
		oss << " polygons";

	return oss.str();
}

Optimized::Point3D& Mesh::Center()
{
	return _center;
}

float Mesh::Diameter()
{
	return _diameter;
}

void Mesh::CopyEssentials( Mesh &mesh )
{
	_vertex_locations = mesh._vertex_locations;
	_vertex_normals = mesh._vertex_normals;
	_vertex_colors = mesh._vertex_colors;
	_triangle_normals = mesh._triangle_normals;
	_quad_normals = mesh._quad_normals;
	_triangles = mesh._triangles;
	_quads = mesh._quads;
	_vertex_users = mesh._vertex_users;
	_filename = mesh._filename;
}

/** Free unimportant mesh data.
 *
 * Free memory by clearing temporary data unimportant to a mesh object.
**/
void Mesh::ClearExtraData()
{
	/*_active_vertex_indices.clear();
	  _active_vertex_distances.clear();*/
	//_modified_vertex_indices.clear();
}

Brush& Mesh::CurrentBrush()
{
	static Brush brush;
	return brush;
}

void Mesh::PerformanceTest( std::string )
{
	CurrentBrush().StartSculpt( *this );
	for( unsigned i = 0; i < 25; ++i )
	{
		_current_selection = _vertex_locations[ rand() % _vertex_locations.size() ];
		CurrentBrush().ContinueSculpt( *this );
	}
	//Unify();

	_change_operation( false, false );
}

/** Loads a mesh in the OBJ format.
 *
 * Loads mesh data from an OBJ file. Support is incomplete; the parsing has
 * bugs, and normal data is ignored.
**/
/*void Mesh::_load_obj( std::string filename )
{
	std::ifstream file( filename.c_str(), std::ios::in );
	//std::istringstream line;
	std::string token, subtoken;
	char buffer[ 256 ];
	unsigned polygon_count = 0;
	unsigned ndx;
	bool hascolor = false;

	_vertex_locations.clear();
	_vertex_normals.clear();
	_vertex_colors.clear();
	_uvs.clear();
	_clear_polys();
	std::vector< Polygon3D > polygons;

	while( !file.eof() )
	{
		std::istringstream line;
		file.getline( buffer, 256 );
		line.str( buffer );
		line >> token;
		if( token == "v" )
		{
			float x, y, z;
			line >> x >> y >> z;
			_vertex_locations.push_back( Point3D( x, y, z ) );
		}
		else if( token == "vt" )
		{
			float u, v;
			line >> u >> v;
			_uvs.push_back( UV( u, v ) );
		}
		else if( token == "f" )
		{
			polygons.resize( polygon_count + 1 );
			while( line >> token )
			{
				int loc = token.find( '/' );
				subtoken = token.substr( 0, loc );
				polygons[ polygon_count ].VertexIndices.push_back( ToInt( subtoken ) - 1 );
			}
			polygon_count++;
		}
		else if( token == "#vertexcolor" )
		{
			line >> ndx;
			_vertex_colors.push_back( SharpConstruct::Color() );
			line >> _vertex_colors[ ndx ].Red >> _vertex_colors[ ndx ].Green >>
			        _vertex_colors[ ndx ].Blue;
			hascolor = true;
		}
	}
	file.close();
	
	_vertex_normals.resize( _vertex_locations.size() );
	
	for( unsigned i = 0; i < polygons.size(); ++i )
	{
		if( polygons[ i ].VertexIndices.size() == 3 )
		{
			_triangles.push_back( Triangle( polygons[ i ].VertexIndices[ 0 ],
			                                polygons[ i ].VertexIndices[ 1 ],
			                                polygons[ i ].VertexIndices[ 2 ] ) );
		}
		else if( polygons[ i ].VertexIndices.size() == 4 )
		{
			_quads.push_back( Quad( polygons[ i ].VertexIndices[ 0 ],
			                        polygons[ i ].VertexIndices[ 1 ],
			                        polygons[ i ].VertexIndices[ 2 ],
			                        polygons[ i ].VertexIndices[ 3 ] ) );
		}
		else
		{
			for( unsigned j = 1; j < polygons[ i ].VertexIndices.size() - 1; ++j )
			{
				_triangles.push_back( Triangle( polygons[ i ].VertexIndices[ 0 ],
				                                polygons[ i ].VertexIndices[ j ],
				                                polygons[ i ].VertexIndices[ j + 1 ] ) );
			}
		}
	}
	
	_triangle_normals.resize( _triangles.size() );
	_quad_normals.resize( _quads.size() );
	
	_recalculate_vertex_users();
	RecalculateAllNormals();
	
	if( !hascolor )
	{
		_vertex_colors.resize( _vertex_locations.size() );
		Flood( 1 );
	}
	
	_saved = true;
}*/

/** Saves a mesh in the OBJ format.
 *
 * Saves mesh data to an OBJ file. Only vertex and polygon data is exported,
 * not normals. The top line is a comment including the SharpConstruct URL.
**/
void Mesh::_save_obj( std::string filename )
{
	std::ofstream file( filename.c_str(), std::ios::out );
	
	unsigned i, j;
	
	file << "# Exported from SharpConstruct " + SharpConstruct::GetVersion() +
	        " - http://sharp3d.sourceforge.net/\n\n";
	
	for( i = 0; i < _vertex_locations.size(); ++i )
		file << "v " << _vertex_locations[ i ].X() << ' ' <<
	                    _vertex_locations[ i ].Y() << ' ' <<
	                    _vertex_locations[ i ].Z() << '\n';
	
	//for( i = 0; i < _vertex_locations.size(); ++i )
	//	file << "vt " << _vertices.U[ i ] << ' ' << _vertices.V[ i ] << '\n';
	
	for( i = 0; i < _triangles.size(); ++i )
	{
		file << "f ";
		for( j = 0; j <= 2; ++j )
			file << _triangles[ i ].V[ j ] + 1 << ' ';		
		file << '\n';
	}
	for( i = 0; i < _quads.size(); ++i )
	{
		file << "f ";
		for( j = 0; j <= 3; ++j )
			file << _quads[ i ].V[ j ] + 1 << ' ';		
		file << '\n';
	}
	
	for( i = 0; i < _vertex_colors.size(); ++i )
		file << "#vertexcolor " << i << ' ' << _vertex_colors[ i ].Red << ' '
		     << _vertex_colors[ i ].Green << ' ' << _vertex_colors[ i ].Blue
		     << '\n';
	
	file.close();
	
	_saved = true;
}

/** Saves a mesh in the VRML97 (.wrl) format.
 *
 * Saves mesh data to an WRL file. Only vertex and polygon data is exported,
 * not normals. The top line is the VRML magic numbers string,
 * followed by a line with a comment including the SharpConstruct URL.
 *
 * Thanks to Joerg Scheurich for this code.
**/
void Mesh::_save_wrl( std::string filename )
{
	std::ofstream file( filename.c_str(), std::ios::out );
	
	unsigned i, j;
	
	file << "#VRML V[ 1 ].0 utf8\n# Exported from SharpConstruct " + SharpConstruct::GetVersion() +
	        " - http://sharp3d.sourceforge.net/\n\n";
	file << "Transform\n";
	file << "  {\n";
	file << "  children\n";
	file << "    [\n";
	file << "    Shape\n";
	file << "      {\n";
	file << "      appearance       Appearance\n";
	file << "        {\n";
	file << "        material        Material\n";
	file << "          {\n";
	file << "          }\n";
	file << "        }\n";
	file << "      geometry       IndexedFaceSet\n";
	file << "        {\n";
	file << "        # double sided\n";
	file << "        solid FALSE\n";
	file << "        # single sided: inside or outside ?\n";
	file << "        #ccw TRUE\n";
	file << "        #ccw FALSE\n";
	file << "        # smooth if angle < 90 degree\n";
	file << "        creaseAngle 1.57\n";
	file << "        coord   Coordinate\n";
	file << "          {\n";
	file << "          point\n";
	file << "            [\n";
	for( i = 0; i < _vertex_locations.size(); ++i )
	{
		file << "            " << 
		_vertex_locations[ i ].X() << ", " <<
		_vertex_locations[ i ].Y() << ", " <<
		_vertex_locations[ i ].Z() << ", \n";
	}
	file << "            ]\n";
	file << "          }\n";
	file << "        coordIndex\n";
	file << "          [\n";
	for( i = 0; i < _triangles.size(); ++i )
	{
		file << "          ";
		for( j = 0; j <= 2; ++j )
			file << _triangles[ i ].V[ j ] << ", ";		
		file << " -1, " << '\n';
	}
	for( i = 0; i < _quads.size(); ++i )
	{
		file << "          ";
		for( j = 0; j <= 3; ++j )
			file << _quads[ i ].V[ j ] << ", ";		
		file << " -1, " << '\n';
	}
	file << "          ]\n";

/* not implemented yet	
for( i = 0; i < _vertices.size(); ++i )
	file << "#vertexcolor " << i << ' ' << _vertices[ i ].R << ' ' <<
			_vertices[ i ].G << ' ' << _vertices[ i ].B << '\n';
*/
	file << "        }\n";
	file << "      }\n";
	file << "    ]\n";
	file << "  }\n";

	file.close();
	
	_saved = true;
}

Point3D Mesh::VertexNeighborLocAverage( unsigned v ) const
{
	Point3D average( 0, 0, 0 );
	unsigned total = 0;
	for( unsigned j = 0; j < _vertex_users[ v ].Triangles.size(); ++j )
	{
		const unsigned polygon_index = _vertex_users[ v ].Triangles[ j ];
		for( unsigned k = 0; k <= 2; k++ )
			if( _triangles[ polygon_index ].V[ k ] == v )
			{
				int previous_point_index = k - 1;
				int next_point_index = k + 1;
				if( previous_point_index < 0 )
					previous_point_index = 2;
				if( next_point_index == 3 )
					next_point_index = 0;
				
				average += _vertex_locations[ v ];
				average += _vertex_locations[ _triangles[ polygon_index ].V[ previous_point_index ] ];
				average += _vertex_locations[ _triangles[ polygon_index ].V[ next_point_index ] ];
				total += 3;
			}
	}
	for( unsigned j = 0; j < _vertex_users[ v ].Quads.size(); ++j )
	{
		const unsigned polygon_index = _vertex_users[ v ].Quads[ j ];
		for( unsigned k = 0; k <= 3; k++ )
			if( _quads[ polygon_index ].V[ k ] == v )
			{
				int previous_point_index = k - 1;
				int next_point_index = k + 1;
				if( previous_point_index < 0 )
					previous_point_index = 3;
				if( next_point_index == 4 )
					next_point_index = 0;
				
				average += _vertex_locations[ v ];
				average += _vertex_locations[ _quads[ polygon_index ].V[ previous_point_index ] ];
				average += _vertex_locations[ _quads[ polygon_index ].V[ next_point_index ] ];
				total += 3;
			}
	}
	
	average /= total;
	
	return average;
}
Color Mesh::VertexNeighborClrAverage( unsigned v ) const
{
	Color average( 0, 0, 0 );
	unsigned total = 0;
	for( unsigned j = 0; j < _vertex_users[ v ].Triangles.size(); ++j )
	{
		const unsigned polygon_index = _vertex_users[ v ].Triangles[ j ];
		for( unsigned k = 0; k <= 2; k++ )
			if( _triangles[ polygon_index ].V[ k ] == v )
			{
				int previous_point_index = k - 1;
				int next_point_index = k + 1;
				if( previous_point_index < 0 )
					previous_point_index = 2;
				if( next_point_index == 3 )
					next_point_index = 0;
				
				average += _vertex_colors[ v ];
				average += _vertex_colors[ _triangles[ polygon_index ].V[ previous_point_index ] ];
				average += _vertex_colors[ _triangles[ polygon_index ].V[ next_point_index ] ];
				total += 3;
			}
	}
	for( unsigned j = 0; j < _vertex_users[ v ].Quads.size(); ++j )
	{
		const unsigned polygon_index = _vertex_users[ v ].Quads[ j ];
		for( unsigned k = 0; k <= 3; k++ )
			if( _quads[ polygon_index ].V[ k ] == v )
			{
				int previous_point_index = k - 1;
				int next_point_index = k + 1;
				if( previous_point_index < 0 )
					previous_point_index = 3;
				if( next_point_index == 4 )
					next_point_index = 0;
				
				average += _vertex_colors[ v ];
				average += _vertex_colors[ _quads[ polygon_index ].V[ previous_point_index ] ];
				average += _vertex_colors[ _quads[ polygon_index ].V[ next_point_index ] ];
				total += 3;
			}
	}
	
	average /= total;
	
	return average;
}

Point3D Mesh::_polygon_center( Triangle& t )
{
	Point3D c( 0, 0, 0 );
	for( unsigned i = 0; i <= 2; ++i )
		c += _vertex_locations[ t.V[ i ] ];
	c /= 3;
	return c;
}
Point3D Mesh::_polygon_center( Quad& q )
{
	Point3D c( 0, 0, 0 );
	for( unsigned i = 0; i <= 3; ++i )
		c += _vertex_locations[ q.V[ i ] ];
	c /= 4;
	return c;
}

/** Calculates center and diameter of sphere formed by vertices.
 *
 * All vertices in the model are used to create a virtual sphere, with a center
 * at the average of all the points and the diameter equal to two times the
 * distance from the center to the furthest point from the center.
**/
void Mesh::CalculateBoundingSphere()
{
	float tmp;

	_diameter = 0;
	
	_center.Zero();

	for( unsigned i = 0; i < _vertex_locations.size(); ++i )
		_center += _vertex_locations[ i ];
	
	_center /= _vertex_locations.size();
	
	for( unsigned i = 0; i < _vertex_locations.size(); ++i )
	{
		tmp = _center.Distance( _vertex_locations[ i ] );
		if( tmp > _diameter )
			_diameter = tmp;
	}
	
	_diameter *= 2;
}

/** Calculates the average normal of the area.
 *
 * Returns the average normal of all the points within the area.
**/
Point3D Mesh::CalculateAreaNormal( const std::vector< ActiveData >& active ) const
{
	Optimized::Point3DVector& norbackup = MeshHistory::Instance().GetPreviousComposite().Vertices()->VertexNormals();

	Normal3D area_normal;
	area_normal.Zero();
		
	for( unsigned i = 0; i < active.size(); ++i )
		area_normal += norbackup[ active[ i ].Index() ];

	Normalize( area_normal );
	
	return area_normal;
}

/** Updates all vertex and polygon normals.
 *
 * Acts like _recalculate_modified_normals(), but does the recalculation for
 * every vertex and every polygon.
 *
 * Note: all polygons are treated as planar
**/
void Mesh::RecalculateAllNormals()
{
	for( unsigned i = 0; i < _triangle_normals.size(); ++i )
		_triangle_normals[ i ].CalculatePlaneUnormal(
			_vertex_locations[ _triangles[ i ].V[ 0 ] ].Data(),
			_vertex_locations[ _triangles[ i ].V[ 1 ] ].Data(),
			_vertex_locations[ _triangles[ i ].V[ 2 ] ].Data() );
	for( unsigned i = 0; i < _quad_normals.size(); ++i )
		_quad_normals[ i ].CalculatePlaneUnormal(
			_vertex_locations[ _quads[ i ].V[ 0 ] ].Data(),
			_vertex_locations[ _quads[ i ].V[ 1 ] ].Data(),
			_vertex_locations[ _quads[ i ].V[ 2 ] ].Data() );

	unsigned j, k;
	for( unsigned i = 0; i < _vertex_users.size(); ++i )
	{
		Normal3D& cur_norm( _vertex_normals[ i ] );
		const PolygonUsers& usr( _vertex_users[ i ] );
		cur_norm.Zero();
		for( j = 0; j < usr.Triangles.size(); ++j )
			cur_norm += _triangle_normals[ usr.Triangles[ j ] ];
		for( k = 0; k < usr.Quads.size(); k++ )
			cur_norm += _quad_normals[ usr.Quads[ k ] ];

		Normalize( cur_norm );
	}
}

void Mesh::ClearModifiedVertexIndices()
{
	//fill( _modified_vertex_indices.begin(), _modified_vertex_indices.end(), false );
	_modified_vertex_indices.clear();
}

void Mesh::RecalculateDamaged()
{
	/* Speed difference is undet. */

	/*_damaged_triangles.resize( VertexLocations().size() );
	_damaged_quads.resize( VertexLocations().size() );

	fill( _damaged_quads.begin(), _damaged_quads.end(), false );
	fill( _damaged_triangles.begin(), _damaged_triangles.end(), false );

	const unsigned mod_size( _modified_vertex_indices.size() );
	for( unsigned i = 0; i < mod_size; ++i )
	{
		const unsigned mod_ndx( _modified_vertex_indices[ i ] );

		for( unsigned j = 0; j < _vertex_users[ mod_ndx ].Triangles.size(); ++j )
			_damaged_triangles[ _vertex_users[ mod_ndx ].Triangles[ j ] ] = true;
		for( unsigned j = 0; j < _vertex_users[ mod_ndx ].Quads.size(); ++j )
			_damaged_quads[ _vertex_users[ mod_ndx ].Quads[ j ] ] = true;
	}

	return;*/

	// This actually works faster with vector than list,
	// and using list with sort+unique is even slower
	// This comment ST approved

	_damaged_triangles.clear();
	_damaged_quads.clear();

#ifdef BM
	for( unsigned i = 0; i < _modified_vertex_indices.size(); ++i )
	{
		if( _modified_vertex_indices[ i ] )
		{
			for( unsigned j = 0; j < _vertex_users[ i ].Quads.size(); ++j )
				_damaged_quads.push_back( _vertex_users[ i ].Quads[ j ] );
		}
	}
#else	
	for( unsigned i = 0; i < _modified_vertex_indices.size(); ++i )
	{
		const unsigned mod_ndx( _modified_vertex_indices[ i ] );

		for( unsigned j = 0; j < _vertex_users[ mod_ndx ].Triangles.size(); ++j )
			_damaged_triangles.push_back( _vertex_users[ mod_ndx ].Triangles[ j ] );
		for( unsigned j = 0; j < _vertex_users[ mod_ndx ].Quads.size(); ++j )
			_damaged_quads.push_back( _vertex_users[ mod_ndx ].Quads[ j ] );
	}
#endif
}

/** Updates modified vertex and polygon normals.
 *
 * Uses the _modified_vertex_indices list to update only modified vertex and
 * polygon normals. This is generally much faster than calling
 * RecalculateAllNormals().
**/
void Mesh::RecalculateModifiedNormals()
{
	unsigned j;
	
	//if( _modified_vertex_indices.size() < _vertex_locations.size() )
	if( 1 )
	{
		const DamagedGroup::iterator t_end( _damaged_triangles.end() );
		for( DamagedGroup::iterator a = _damaged_triangles.begin();
		     a != t_end; ++a )
		{
			const unsigned ndx( *a );
			const Triangle& curt( _triangles[ ndx ] );
			_triangle_normals[ ndx ].CalculatePlaneUnormal(
				_vertex_locations[ curt.V[ 0 ] ].Data(),
				_vertex_locations[ curt.V[ 1 ] ].Data(),
				_vertex_locations[ curt.V[ 2 ] ].Data() );
		}

		const DamagedGroup::iterator q_end( _damaged_quads.end() );
		for( DamagedGroup::iterator a = _damaged_quads.begin();
		     a != q_end; ++a )
		{
			const unsigned ndx( *a );
			const Quad& curq( _quads[ ndx ] );
			_quad_normals[ ndx ].CalculatePlaneUnormal(
				_vertex_locations[ curq.V[ 0 ] ].Data(),
				_vertex_locations[ curq.V[ 1 ] ].Data(),
				_vertex_locations[ curq.V[ 2 ] ].Data() );
		}

		for( unsigned i = 0; i < _modified_vertex_indices.size(); ++i )
		{
			const unsigned mod_ndx( _modified_vertex_indices[ i ] );
			Normal3D& cur_norm( _vertex_normals[ mod_ndx ] );
			const PolygonUsers& usr( _vertex_users[ mod_ndx ] );

			cur_norm.Zero();
			for( j = 0; j < usr.Triangles.size(); ++j )
				cur_norm += _triangle_normals[ usr.Triangles[ j ] ];
			for( j = 0; j < usr.Quads.size(); ++j )
				cur_norm += _quad_normals[ usr.Quads[ j ] ];

			Normalize( cur_norm );
		}
	}
	else
		RecalculateAllNormals();

	/*for( unsigned i = 0; i < _modified_vertex_indices.size(); ++i )
	{
		const unsigned current( _modified_vertex_indices[ i ] );
		const std::set< unsigned > cvs( _connected_vertices( current ) );
		_vertex_normals[ current ].CalculatePlaneNormal
			( _vertex_locations[ *( ++( ++cvs.begin() ) ) ].RawData(),
			  _vertex_locations[ *cvs.begin() ].RawData(),
			  _vertex_locations[ *++cvs.begin() ].RawData() );
		//_vertex_normals[ current ] = Normal3D( 0, 1, 0 );
	}*/

	_change_operation( false, false );
}

void Mesh::_recalculate_vertex_users()
{
	_vertex_users.clear();
	_vertex_users.resize( _vertex_locations.size() );
	for( unsigned i = 0; i < _triangles.size(); ++i )
	{
		const Triangle& t( _triangles[ i ] );
		_vertex_users[ t.V[ 0 ] ].Triangles.push_back( i );
		_vertex_users[ t.V[ 1 ] ].Triangles.push_back( i );
		_vertex_users[ t.V[ 2 ] ].Triangles.push_back( i );
	}
	for( unsigned i = 0; i < _quads.size(); ++i )
	{
		const Quad& q( _quads[ i ] );
		_vertex_users[ q.V[ 0 ] ].Quads.push_back( i );
		_vertex_users[ q.V[ 1 ] ].Quads.push_back( i );
		_vertex_users[ q.V[ 2 ] ].Quads.push_back( i );
		_vertex_users[ q.V[ 3 ] ].Quads.push_back( i );
	}
}

std::map< unsigned, unsigned > Mesh::_triangle_users( unsigned v1, unsigned v2 )
{
	std::map< unsigned, unsigned > r;
	// For every user of v1
	for( unsigned i = 0; i < _vertex_users[ v1 ].Triangles.size(); ++i )
	{
		// For every user of v2
		for( unsigned j = 0; j < _vertex_users[ v2 ].Triangles.size(); ++j )
		{
			const unsigned poly = _vertex_users[ v1 ].Triangles[ i ];
			// If poly is a user of both v2 (as well as v1)
			if( poly == _vertex_users[ v2 ].Triangles[ j ] )
			{
				unsigned w = 0;
				for( unsigned k = 0; k < 3; k++ )
				{
					// Return the side (1, 2, or 3) that's connected
					
					if( _triangles[ poly ].V[ k ] != v1 &&
					    _triangles[ poly ].V[ k ] != v2 )
					{
						w = k + 2;
						if( k == 2 )
							w = 1;
						break;
					}
				}
				r[ poly] = w;
				break;
			}
		}
	}
	
	return r;
}
std::map< unsigned, unsigned > Mesh::_quad_users( unsigned v1, unsigned v2 )
{
	std::map< unsigned, unsigned > r;
	// For every user of v1
	for( unsigned i = 0; i < _vertex_users[ v1 ].Quads.size(); ++i )
	{
		// For every user of v2
		for( unsigned j = 0; j < _vertex_users[ v2 ].Quads.size(); ++j )
		{
			const unsigned poly = _vertex_users[ v1 ].Quads[ i ];
			// If poly is a user of both v2 (as well as v1)
			if( poly == _vertex_users[ v2 ].Quads[ j ] )
			{
				unsigned w = 0;
				for( unsigned k = 0; k < 4; k++ )
				{
					unsigned k2 = k + 1;
					if( k2 > 3 )
						k2 = 0;
					if( ( _quads[ poly ].V[ k ] == v1 &&
					      _quads[ poly ].V[ k2 ] == v2 ) ||
					    ( _quads[ poly ].V[ k ] == v2 &&
					      _quads[ poly ].V[ k2 ] == v1 ) )
					{
						w = k + 1;
						break;
					}
				}
				r[ poly] = w;
				break;
			}
		}
	}
	
	return r;
}

unsigned Mesh::_poly_count() const
{
	return _triangles.size() + _quads.size();
}

void Mesh::_clear_polys()
{
	_triangles.clear();
	_quads.clear();
	_triangle_normals.clear();
	_quad_normals.clear();
}

void Mesh::_reset_visibles()
{
	_visible_elements.Vertices = _vertex_locations.size();
	_visible_elements.Triangles = _triangles.size();
	_visible_elements.Quads = _quads.size();
}

std::set< unsigned > Mesh::_connected_polygon_vertices( unsigned v ) const
{
	std::set< unsigned > r;

	for( unsigned j = 0; j < _vertex_users[ v ].Triangles.size(); ++j )
	{
		for( unsigned k = 0; k <= 2; ++k )
		{
			const unsigned index = _triangles[ _vertex_users[ v ].Triangles[ j ] ].V[ k ];
			if( index != v )
				r.insert( index );
		}
	}
	for( unsigned j = 0; j < _vertex_users[ v ].Quads.size(); ++j )
	{
		for( unsigned k = 0; k <= 3; ++k )
		{
			const unsigned index = _quads[ _vertex_users[ v ].Quads[ j ] ].V[ k ];
			if( index != v )
				r.insert( index );
		}
	}
	return r;
}

std::set< unsigned > Mesh::_connected_vertices( unsigned v ) const
{
	std::set< unsigned > r;
	
	for( unsigned j = 0; j < _vertex_users[ v ].Triangles.size(); ++j )
	{
		for( unsigned k = 0; k <= 2; ++k )
		{
			if( _triangles[ _vertex_users[ v ].Triangles[ j ] ].V[ k ] == v )
			{
				const unsigned add( k + 1 );

				r.insert( _triangles[ _vertex_users[ v ].Triangles[ j ] ].V[ k - 1 < 0 ? 2 : k - 1 ] );
				r.insert( _triangles[ _vertex_users[ v ].Triangles[ j ] ].V[ add > 2 ? 0 : add ] );

				break;
			}
		}
	}
	for( unsigned j = 0; j < _vertex_users[ v ].Quads.size(); ++j )
	{
		for( unsigned k = 0; k <= 3; ++k )
		{
			if( _quads[ _vertex_users[ v ].Quads[ j ] ].V[ k ] == v )
			{
				const unsigned add( k + 1 );

				r.insert( _quads[ _vertex_users[ v ].Quads[ j ] ].V[ k - 1 < 0 ? 3 : k - 1 ] );
				r.insert( _quads[ _vertex_users[ v ].Quads[ j ] ].V[ add > 3 ? 0 : add ] );

				break;
			}
		}
	}
	
	return r;
}

/** Stores vertices within the MainBrush->GetRadius().
 *
 * Stores a list of all vertices within the sphere that has its center at area
 * and a radius of MainBrush->GetRadius(). The distance from the active vertices
 * to the center is also stored.
**/
// Slow
/*void Mesh::_find_active_vertices( std::vector< unsigned > &vertex_indices,
                                  std::vector< float > &vertex_distances,
                                  EditData e )
{
	vertex_indices.clear();
	vertex_distances.clear();
	
	float distance;
	
	for( int i = 0; i < _visible_elements.Vertices; ++i )
	{
		distance = e.Center.Distance( _vertex_locations[ i ] );
		if( distance < _selection_radius )
		{
			vertex_indices.push_back( i );
			vertex_distances.push_back( distance );
		}
	}
	
	_selected_vertex[ e.SIndex ] = vertex_indices[ 0 ];

	_modified_vertex_indices.insert( _modified_vertex_indices.end(),
					 vertex_indices.begin(),
	                                 vertex_indices.end() );
}*/
/* This version of _find_active_vertices may be rather slow too...*/
/*void Mesh::_find_active_vertices( std::vector< unsigned > &vertex_indices,
                                  std::vector< float > &vertex_distances,
                                  EditData e )
{	
	vertex_indices.clear();
	vertex_distances.clear();
	
	int area_index = 0;

	if( _selected_vertex[ e.SIndex ] >= _vertex_locations.size() )
		_selected_vertex[ e.SIndex ] = 0;

	// If a point used last time through the loop is within the Radius, use it!
	if( _vertex_locations[ _selected_vertex[ e.SIndex ] ].Distance( e.Center ) <
	    _selection_radius )
	{
		area_index = _selected_vertex[ e.SIndex ];
	}
	else
	{
		float tmp_distance;
		
		// The point of this loop is just to get any vertex that is within the Radius
		for( int i = 0; i < _vertex_locations.size(); ++i )
		{
			e.Center.FastDistance( _vertex_locations[ i ], tmp_distance );

			if( tmp_distance < _selection_radius )
			{
				area_index = i;
				break;
			}
		}
	}

	std::set< unsigned > checked;
	ActiveVertexData d( area_index, checked, vertex_indices, vertex_distances, e.Center );
	_is_vertex_active( d );
	
	_selected_vertex[ e.SIndex ] = vertex_indices[ 0 ];

	_modified_vertex_indices.insert( _modified_vertex_indices.end(),
					 vertex_indices.begin(),
	                                 vertex_indices.end() );
}*/

/*void Mesh::_is_vertex_active( ActiveVertexData& d )
{
	unsigned current = d.Current;

	int xdf = d.Checked.size();
	d.Checked.insert( current );
	
	float distance = d.Area.Distance( _vertex_locations[ current ] );
	if( distance < _selection_radius )
	{
		if( current < _lowest_index )
			_lowest_index = current;
		if( current > _highest_index )
			_highest_index = current;
		d.VertexIndices.push_back( current );
		d.VertexDistances.push_back( distance );
	}
	else
		return;
	
	int j, polygon_index;
	int previous_point_index;
	// Loop through each triangle using the current point
	for( unsigned i = 0; i < _vertex_users[ current ].Triangles.size(); ++i )
	{
		// Set the current polygon
		polygon_index =_vertex_users[ current ].Triangles[ i ];
		// Loop through each vertex in the polygon
		for( j = 0; j <= 2; ++j )
			// If the current active point equals the current VertexIndex...
			if( _triangles[ polygon_index ].V[ j ] == current )
			{
				previous_point_index = j - 1;
				if( previous_point_index < 0 )
					previous_point_index = 2;

				previous_point_index = _triangles[ polygon_index ].V[ previous_point_index ];

				if( d.Checked.find( previous_point_index ) == d.Checked.end() )
				{
					d.Current = previous_point_index;
					_is_vertex_active( d );
				}
				break;
			}
	}
	
	// Loop through each quad using the current point
	for( unsigned i = 0; i < _vertex_users[ current ].Quads.size(); ++i )
	{
		// Set the current polygon
		polygon_index =_vertex_users[ current ].Quads[ i ];
		// Loop through each vertex in the polygon
		for( j = 0; j <= 3; ++j )
			// If the current active point equals the current VertexIndex...
			if( _quads[ polygon_index ].V[ j ] == current )
			{
				previous_point_index = j - 1;
				if( previous_point_index < 0 )
					previous_point_index = 3;

				previous_point_index = _quads[ polygon_index ].V[ previous_point_index ];

				if( d.Checked.find( previous_point_index ) == d.Checked.end() )
				{
					d.Current = previous_point_index;
					_is_vertex_active( d );
				}
				break;
			}
	}
}
*/