Mesh::Mesh(VertexList const& vertices)
: m_id(afth::UUID::v4())
{
    size_t size = vertices.size() * 3;
    float* arr = new float[size];
    VertexList::const_iterator it = vertices.begin(), end = vertices.end();
    for (size_t i = 0; it != end; ++it, i += 3)
    {
        std::memcpy(arr + i, (*it).coordinates().arr().data(), 3 * sizeof(float));
    }

    //glGenVertexArrays(1, &m_vertexArray);
    //glGenBuffers(1, &m_vertexBuffer);

    //glBindBuffer(GL_ARRAY_BUFFER, m_vertexBuffer);
    //glBufferData(GL_ARRAY_BUFFER, size * sizeof(float), arr, GL_STATIC_DRAW);

    //glBindVertexArray(m_vertexArray);

    //GLint positionIndex = glGetAttribLocation(glProgramUniform1, "position");
    //glEnableVertexAttribArray(0);
    //glVertexAttribPointer(0, 3, GL_FLOAT, GL_FALSE, 0, 0);

    //glBindBuffer(GL_ARRAY_BUFFER, 0);
    //glBindVertexArray(0);

    delete [] arr;
}

void Cylinder::buildTopCap(VertexList& vertices, IndexList& indices)
{
	UINT baseIndex = (UINT)vertices.size();

	// Duplicate cap vertices because the texture coordinates and normals differ.
	float y = 0.5f*mHeight;

	// vertices of ring
	float dTheta = 2.0f*PI/mNumSlices;
	for(UINT i = 0; i <= mNumSlices; ++i)
	{
		float x = mTopRadius*cosf(i*dTheta);
		float z = mTopRadius*sinf(i*dTheta);

		// Map [-1,1]-->[0,1] for planar texture coordinates.
		float u = +0.5f*x/mTopRadius + 0.5f;
		float v = -0.5f*z/mTopRadius + 0.5f;

		vertices.push_back( Vertex(x, y, z, 1.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, u, v) );
	}

	// cap center vertex
	vertices.push_back( Vertex(0.0f, y, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.5f, 0.5f) );

	// index of center vertex
	UINT centerIndex = (UINT)vertices.size()-1;

	for(UINT i = 0; i < mNumSlices; ++i)
	{
		indices.push_back(centerIndex);
		indices.push_back(baseIndex + i+1);
		indices.push_back(baseIndex + i);
	}
}

Vector ClosestPointPoly ( Vector const & V, VertexList const & vertices )
{
	if(vertices.size() == 0) return V;

	if(vertices.size() == 1) return vertices[0];

	if(vertices.size() == 2) return ClosestPointSeg( V, Segment3d(vertices[0],vertices[1]) );

	// ----------

	Vector closest = ClosestPointTri( V, Triangle3d(vertices[0],vertices[1],vertices[2]) );

	int nTris = vertices.size() - 2;

	for(int i = 1; i < nTris; i++)
	{
		Triangle3d tri( vertices[0], vertices[i+1], vertices[i+2] );

		Vector temp = ClosestPointTri(V,tri);

		closest = selectCloser(V,closest,temp);
	}

	return closest;
}

void ConeRenderer::init(ShaderProgram* prog)
{
	if(_vbo) {
	    _vbo->bind();
	    _vbo->setPointer();
	    return;
	}
	_vbo = make_resource<VBO>(getResourceManager(),
	                          "P");
	_vbo->overrideIndex(getResourceManager()->geometryCache()->getIndexForAttribute("P"));
	_vbo->bind();
	prog->bindAttributeLocation(_vbo.get());

	VertexList verts;

    //cone
    verts.push_back(glm::vec3(0, 1, 0));
    for(int i=0; i <= _segments; i++) {
        float pni = 2 * PI * float(i) / _segments;
        verts.push_back(glm::vec3(sin(pni), 0, cos(pni)));
    }
    //disc
    verts.push_back(glm::vec3());
    for(int i=_segments; i >= 0; i--) {
        float pni = 2 * PI * float(i) / _segments;
        verts.push_back(glm::vec3(sin(pni), 0, cos(pni)));
    }

    _vbo->data(verts);
    _vbo->setPointer();
}

void IncrementalDelaunayTriangulator::insertSites(const VertexList& vertices)
{
	for (VertexList::const_iterator x=vertices.begin(); 
			x != vertices.end(); ++x) {
		insertSite(*x);
	}
}

void Reprojector::projectCloud( IplImage *image )
{
	cvZero( image );

	if( !this->projVertices.size() )
		return;

	VertexList tempList;
	tempList.resize( this->projVertices.size() );

	transform3DTo2D( this->projVertices.size(), &( this->projVertices[0] ), &( tempList[0] ), this->cxProjector, this->cyProjector, this->focalLengthProjector, false );

	unsigned short val = 0;
	unsigned short *tempPtr = NULL;

	for( VertexList::const_iterator it = tempList.begin(); it != tempList.end(); ++it )
	{
		unsigned int x = it->X;
		unsigned int y = it->Y;
		unsigned short depth = -( it->Z );

		if( x >= image->width || y >= image->height )
			continue;

		val = depth;
		tempPtr = (unsigned short*)( image->imageData ) + x + y * image->width;

		if( !( *tempPtr ) || *tempPtr > val )	//z-test
			*tempPtr = val;
	}
}

Drawable RenderingEngine::CreateDrawable(const ParametricSurface& surface, int flags) const
{
    // Create the VBO for the vertices.
    VertexList vertices;
    surface.GenerateVertices(vertices, flags);
    GLuint vertexBuffer;
    glGenBuffers(1, &vertexBuffer);
    glBindBuffer(GL_ARRAY_BUFFER, vertexBuffer);
    glBufferData(GL_ARRAY_BUFFER,
                 vertices.size() * sizeof(vertices[0]),
                 &vertices[0],
                 GL_STATIC_DRAW);
    
    // Create a new VBO for the indices if needed.
    int indexCount = surface.GetTriangleIndexCount();
    GLuint indexBuffer;
    IndexList indices(indexCount);
    surface.GenerateTriangleIndices(indices);
    glGenBuffers(1, &indexBuffer);
    glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, indexBuffer);
    glBufferData(GL_ELEMENT_ARRAY_BUFFER,
                 indexCount * sizeof(GLushort),
                 &indices[0],
                 GL_STATIC_DRAW);
    
    // Fill in a descriptive struct and return it.
    Drawable drawable;
    drawable.IndexBuffer = indexBuffer;
    drawable.VertexBuffer = vertexBuffer;
    drawable.IndexCount = indexCount;
    drawable.Flags = flags;
    return drawable;
}

void print_out (VertexList V)
{
   // print out
   for (VertexList::iterator i = V.begin(); i != V.end(); ++i)
   {
      cout << i->first << " spe:" << i->second.spe << " pi:" << i->second.pi << endl;
   }
}

iGraphic* CreatePath(float fromx,float fromy, float fromz, 
		   float tox,float toy, float toz){
    VertexList<Vertex>* vertexList = 
     (VertexList<Vertex>*)CreateVertexList<Vertex>(LINE_LIST, 1);
        vertexList->add(Vertex(Vector((float)fromx, (float)fromy, (float)fromz),Vector( 0, 1, 0)));
		vertexList->add(Vertex(Vector((float)tox, (float)toy, (float)toz),Vector( 0, 1, 0)));
	return vertexList;
}

	void VertexListToBufP(std::vector<float> &dst,const VertexList &list){
		dst.clear();
		dst.reserve(list.size()*3);
		for(VertexList::const_iterator i = list.begin();i!=list.end();i++){
			dst.push_back((*i).pos.x);
			dst.push_back((*i).pos.y);
			dst.push_back((*i).pos.z);
		}
	}

void copyPointListToVertexList(const PointList& in,VertexList& out)
{
    out.reserve(in.size());
    for(PointList::const_iterator itr=in.begin();
        itr!=in.end();
        ++itr)
    {
        out.push_back(itr->second);
    }
}

// copyVertexListToPointList a vector for Vec3 into a vector of Point's.
void copyVertexListToPointList(const VertexList& in,PointList& out)
{
    out.reserve(in.size());
    for(VertexList::const_iterator itr=in.begin();
        itr!=in.end();
        ++itr)
    {
        out.push_back(Point(0,*itr));
    }
}

void PolyMesh::findNeighbors( osg::Vec3 p, VertexList& vlist )
{
    for ( EdgeMap::iterator itr=_edges.begin(); itr!=_edges.end(); ++itr )
    {
        if ( equivalent(itr->first.first,p) )
            vlist.push_back( itr->first.second );
        else if ( equivalent(itr->first.second,p) )
            vlist.push_back( itr->first.first );
    }
}

VertexList<Vertex>* Mesh::build(bool normalize) {
  if (normalize) normalizeSize(buildScale);
  VertexList<Vertex>* vertexList = (VertexList<Vertex>*) 
    CreateVertexList<Vertex>(TRIANGLE_LIST, numberOfTriangles());
  for (unsigned i=0; i<faces.size(); ++i)
    addFace(vertexList, faces[i]);
  vertexList->calcAABB();
  cachedMesh = vertexList;
  return vertexList;
}

//***************************************************************************************
// Name: Subdivide
// Desc: Function subdivides every input triangle into four triangles of equal area.
//***************************************************************************************
void Subdivide(VertexList& vertices, IndexList& indices)
{
	VertexList vin = vertices;
	IndexList  iin = indices;

	vertices.resize(0);
	indices.resize(0);

	//       v1
	//       *
	//      / \
	//     /   \
	//  m0*-----*m1
	//   / \   / \
	//  /   \ /   \
	// *-----*-----*
	// v0    m2     v2

	UINT numTris = (UINT)iin.size()/3;
	for(UINT i = 0; i < numTris; ++i)
	{
		D3DXVECTOR3 v0 = vin[ iin[i*3+0] ];
		D3DXVECTOR3 v1 = vin[ iin[i*3+1] ];
		D3DXVECTOR3 v2 = vin[ iin[i*3+2] ];

		D3DXVECTOR3 m0 = 0.5f*(v0 + v1);
		D3DXVECTOR3 m1 = 0.5f*(v1 + v2);
		D3DXVECTOR3 m2 = 0.5f*(v0 + v2);

		vertices.push_back(v0); // 0
		vertices.push_back(v1); // 1
		vertices.push_back(v2); // 2
		vertices.push_back(m0); // 3
		vertices.push_back(m1); // 4
		vertices.push_back(m2); // 5
 
		indices.push_back(i*6+0);
		indices.push_back(i*6+3);
		indices.push_back(i*6+5);

		indices.push_back(i*6+3);
		indices.push_back(i*6+4);
		indices.push_back(i*6+5);

		indices.push_back(i*6+5);
		indices.push_back(i*6+4);
		indices.push_back(i*6+2);

		indices.push_back(i*6+3);
		indices.push_back(i*6+1);
		indices.push_back(i*6+4);
	}
}

bool PLY2Reader::loadPLY2Mesh(std::shared_ptr<Shape> shape, std::string fname)
{
  std::ifstream f_handler(fname);
  if (!f_handler)
  {
    std::cout << "Open file failed.\n";
    return false;
  }

  std::string line;
  std::stringstream ss;

  int num_vertex = 0;
  int num_face = 0;
  VertexList vertexList;
  FaceList faceList;
  STLVectorf UVList;

  getline(f_handler, line);
  ss.str(line);
  ss >> num_vertex;
  getline(f_handler, line);
  ss.str(line);
  ss >> num_face;

  for (int i = 0; i < num_vertex; ++i)
  {
    float v;
    for (int j = 0; j < 3; ++j)
    {
      getline(f_handler, line);
      ss.str(line);
      ss >> v;
      vertexList.push_back(v);
    }
  }

  for (int i = 0; i < num_face; ++i)
  {
    int f;
    for (int j = 0; j < 3; ++j)
    {
      getline(f_handler, line);
      ss.str(line);
      ss >> f;
      faceList.push_back(f);
    }
  }

  shape->init(vertexList, faceList, UVList);
  return true;
}

NodePtr KDTree::makeTree(size_t depth, const size_t& cellSize, VertexLists& t,
        const Domain& domain){
    /*
     * Tuple contains x, y, z  Dimensions Vertex list
     *
    */
    const size_t k = depth % m_K;
    VertexList vertices = t.at(k);

    if(vertices.size() == 0){
        return nullptr;
    }
    if(vertices.size() <= cellSize){
        return NodePtr(new Node(vertices, domain));
    }

    size_t median = (int) (vertices.size()-1)/2;
    VertexPtr& posElement = vertices.at(median);

    //Split lists by median element
    std::vector< ListPair > pairs;
    for(size_t i=0; i<m_K; ++i){
        pairs.push_back(splitListBy(k, t.at(i), posElement));
    }

    VertexLists left;
    VertexLists right;
    for(ListPair pair: pairs){
        left.push_back(std::get<0>(pair));
        right.push_back(std::get<1>(pair));
    }

    Domain leftBounds = domain;
    Domain rightBounds = domain;
    leftBounds.updateMax((*posElement)[k], k);
    rightBounds.updateMin((*posElement)[k], k);

    NodePtr leftNode;
    NodePtr rightNode;
    if(depth < 2){
    thread lT([&] { leftNode = makeTree(depth+1, cellSize, left, leftBounds); });
    thread rT([&] { rightNode = makeTree(depth+1, cellSize, right, rightBounds); });
    lT.join();
    rT.join();
    }else{
        leftNode = makeTree(depth+1, cellSize, left, leftBounds);
        rightNode = makeTree(depth+1, cellSize, right, rightBounds);
    }
    return NodePtr(new Node(leftNode, rightNode, domain));
};

ListPair KDTree::splitListBy(const size_t& index, const VertexList& sourceList,
        const VertexPtr& sourceVert){
    VertexList left;
    VertexList right;

    for(VertexPtr elem : sourceList){
        if((*elem)[index] < (*sourceVert)[index]){
            left.push_back(elem);
        }else{
            right.push_back(elem);
        }
    }
    return ListPair(left, right);
}

VertexList KDTree::findKNearestNeighbours(const VertexPtr source,
        const size_t numNeighbours){
    VertexList result;
    //Stopwatch findS("NKSearch");
    LimitedPriorityQueue resultQueue(numNeighbours);
    findKNearestNeighbours(m_root, resultQueue, source);
    while(!resultQueue.empty()){
        VertexDistPair pair = resultQueue.top();
        VertexPtr vrtx = std::get<0>(pair);
        result.push_back(vrtx);
        resultQueue.pop();
    }
    //findS.stop();
    return result;
}

//***************************************************************************************
// Name: BuildGeoSphere
// Desc: Function approximates a sphere by tesselating an icosahedron.
//***************************************************************************************
void BuildGeoSphere(UINT numSubdivisions, float radius, VertexList& vertices, IndexList& indices)
{
	// Put a cap on the number of subdivisions.
	numSubdivisions = Min(numSubdivisions, UINT(5));

	// Approximate a sphere by tesselating an icosahedron.

	const float X = 0.525731f; 
	const float Z = 0.850651f;

	D3DXVECTOR3 pos[12] = 
	{
		D3DXVECTOR3(-X, 0.0f, Z),  D3DXVECTOR3(X, 0.0f, Z),  
		D3DXVECTOR3(-X, 0.0f, -Z), D3DXVECTOR3(X, 0.0f, -Z),    
		D3DXVECTOR3(0.0f, Z, X),   D3DXVECTOR3(0.0f, Z, -X), 
		D3DXVECTOR3(0.0f, -Z, X),  D3DXVECTOR3(0.0f, -Z, -X),    
		D3DXVECTOR3(Z, X, 0.0f),   D3DXVECTOR3(-Z, X, 0.0f), 
		D3DXVECTOR3(Z, -X, 0.0f),  D3DXVECTOR3(-Z, -X, 0.0f)
	};

	DWORD k[60] = 
	{
		1,4,0,  4,9,0,  4,5,9,  8,5,4,  1,8,4,    
		1,10,8, 10,3,8, 8,3,5,  3,2,5,  3,7,2,    
		3,10,7, 10,6,7, 6,11,7, 6,0,11, 6,1,0, 
		10,1,6, 11,0,9, 2,11,9, 5,2,9,  11,2,7 
	};

	vertices.resize(12);
	indices.resize(60);

	for(int i = 0; i < 12; ++i)
		vertices[i] = pos[i];

	for(int i = 0; i < 60; ++i)
		indices[i] = k[i];

	for(UINT i = 0; i < numSubdivisions; ++i)
		Subdivide(vertices, indices);

	// Project vertices onto sphere and scale.
	for(size_t i = 0; i < vertices.size(); ++i)
	{
		D3DXVec3Normalize(&vertices[i], &vertices[i]);
		vertices[i] *= radius;
	}
}