void ofxPolylineLoad(ofPolyline & poly, string xmlPath) {
    ofXml xml;
    bool bLoaded = xml.load(xmlPath);
    if(bLoaded == false) {
        return;
    }
    
    xml.setTo("poly");
    bool bClosed = ofToInt(xml.getAttribute("closed"));
    
    poly.clear();
    
    int numOfPoints = xml.getNumChildren();
    for(int i=0; i<numOfPoints; i++) {
        xml.setToChild(i);
        float x = ofToFloat(xml.getAttribute("x"));
        float y = ofToFloat(xml.getAttribute("y"));
        
        poly.addVertex(x, y);
    }
    
    if(bClosed == true) {
        poly.close();
    }
}

//--------------------------------------------------------------
void drawWithNormals(const ofPolyline& polyline, int zeroX, int zeroY,
		bool drawContours) {
	for (int i = 0; i < (int) polyline.size(); i++) {
		bool repeatNext = i == (int) polyline.size() - 1;

		const ofPoint& cur = polyline[i];
		const ofPoint& next = repeatNext ? polyline[0] : polyline[i + 1];

		float angle = atan2f(next.y - cur.y, next.x - cur.x) * RAD_TO_DEG;
		float distance = cur.distance(next);

		if (repeatNext) {
			ofSetColor(255, 0, 255);
		}
		glPushMatrix();
		glTranslatef(cur.x + zeroX, cur.y + zeroY, 0);
		ofRotate(angle);
//		ofLine(0, 0, 0, distance);
		ofLine(0, 0, distance, 0);
		ofLine(0, distance, distance, distance);
		if (drawContours) {
			for (int i = distance; i < distance * 3; i += 5) {
				ofLine(0, 0, i, 0);
				ofLine(0, i, i, i);
			}
		}
		glPopMatrix();
	}
}

ofPolyline ofGetSmoothed(const ofPolyline& polyline, int smoothingSize, float smoothingShape) {
	ofPolyline result = polyline;
	
	if(!polyline.getClosed()) {
		ofLog( OF_LOG_ERROR, "ofSmooth() currently only supports closed ofPolylines." );
		return polyline;
	}
	
	// precompute weights and normalization
	vector<float> weights;
	float weightSum = 0;
	weights.push_back(1); // center weight
	// side weights
	for(int i = 1; i <= smoothingSize; i++) {
		float curWeight = ofMap(i, 0, smoothingSize, 1, smoothingShape);
		weights.push_back(curWeight);
		weightSum += curWeight;
	}
	float weightNormalization = 1 / (1 + 2 * weightSum);
	
	// use weights to make weighted averages of neighbors
	int n = polyline.size();
	for(int i = 0; i < n; i++) {
		for(int j = 1; j <= smoothingSize; j++) {
			int leftPosition = (n + i - j) % n;
			int rightPosition = (i + j) % n;
			const ofPoint& left = polyline[leftPosition];
			const ofPoint& right = polyline[rightPosition];
			result[i] += (left + right) * weights[j];
		}
		result[i] *= weightNormalization;
	}
	
	return result;
}

void testApp::renderOutlinesToFBO(ofPolyline& leftEye, ofPolyline& rightEye, ofPolyline& faceOutline){
    outputFbo.begin();
    ofClear(0,0,0,0);
    
    ofPushStyle();
    
    //ofEnableAlphaBlending();
    ofEnableBlendMode(OF_BLENDMODE_ADD);
    
    ofSetColor(0,0,255,255);
    ofBeginShape();
    ofVertices(faceOutline.getVertices());
    ofEndShape();
    
    ofSetColor(255,0,0,255);
    ofBeginShape();
    ofVertices(leftEye.getVertices());
    ofEndShape();
    
    ofSetColor(255,0,0,255);
    ofBeginShape();
    ofVertices(rightEye.getVertices());
    ofEndShape();
    
    ofPopStyle();
    
    outputFbo.end();
}

void contourToConvexHull(ofPolyline &src, ofPolyline &dst) {

    dst.clear();

    vector<hPoint> P(src.size());
    for(int i = 0; i < src.size(); i++) {
        P[i].x = src[i].x;
        P[i].y = src[i].y;
    }

    int n = src.size(), k = 0;
    vector<hPoint> H(2*n);

    // Sort points lexicographically
    sort(P.begin(), P.end());

    // Build lower hull
    for (int i = 0; i < n; i++) {
        while (k >= 2 && cross(H[k-2], H[k-1], P[i]) <= 0) k--;
        H[k++] = P[i];
    }

    // Build upper hull
    for (int i = n-2, t = k+1; i >= 0; i--) {
        while (k >= t && cross(H[k-2], H[k-1], P[i]) <= 0) k--;
        H[k++] = P[i];
    }

    H.resize(k);

    for(int i = 0; i < H.size(); i++) {
        dst.addVertex(H[i].x + 500, H[i].y);
    }
}

void ofxPolyline2Mesh::updateShape(const ofPolyline &polyline)
{
	shape.clear();
	norm.clear();

	for (int i = 0; i < polyline.size(); i++)
	{
		shape.push_back(polyline[i]);
	}

	if (polyline.isClosed())
		shape.push_back(polyline[0]);

	const ofVec3f V(0, 0, -1);

	for (int i = 0; i < shape.size() - 1; i++)
	{
		const ofVec3f& p1 = shape[i];
		const ofVec3f& p2 = shape[i + 1];
		const ofVec3f& n21 = (p2 - p1).normalized();

		norm.push_back(n21.crossed(V));
	}

	{
		const ofVec3f& p1 = shape[shape.size() - 1];
		const ofVec3f& p2 = shape[0];
		const ofVec3f& n21 = (p2 - p1).normalized();

		norm.push_back(n21.crossed(V));
	}

	current_segments.resize(shape.size());
	last_segments.resize(shape.size());
}

ofPolyline ofGetResampledSpacing(const ofPolyline& polyline, float spacing) {
	ofPolyline result;
	// if more properties are added to ofPolyline, we need to copy them here
	result.setClosed(polyline.getClosed());

	float totalLength = 0;
	int curStep = 0;
	int lastPosition = polyline.size() - 1;
	if(polyline.getClosed()) {
		lastPosition++;
	}
	for(int i = 0; i < lastPosition; i++) {
		bool repeatNext = i == (int) (polyline.size() - 1);
	
		const ofPoint& cur = polyline[i];
		const ofPoint& next = repeatNext ? polyline[0] : polyline[i + 1];
		ofPoint diff = next - cur;
		
		float curSegmentLength = diff.length();
		totalLength += curSegmentLength;
		
		while(curStep * spacing <= totalLength) {
			float curSample = curStep * spacing;
			float curLength = curSample - (totalLength - curSegmentLength);
			float relativeSample = curLength / curSegmentLength;
			result.addVertex(cur.getInterpolated(next, relativeSample));
			curStep++;
		}
	}
	
	return result;
}

void testApp::oscSendContour(int label, const ofPolyline &polyline){
    ofxOscMessage m;
    stringstream ss;
    ss<<"/contour";
    m.setAddress(ss.str());
    
    int size = polyline.size();
    m.addIntArg(label);
    m.addIntArg(size);
    cout<<"contour: "<<label<<" size: "<<size<<endl;
    const ofRectangle& rect = polyline.getBoundingBox();
    m.addIntArg(rect.getTopLeft().x);
    m.addIntArg(rect.getTopLeft().y);
    m.addIntArg(rect.getBottomRight().x);
    m.addIntArg(rect.getBottomRight().y);
    ofPolyline newLine = polyline.getResampledByCount(100);
    cout<<"resized to "<<newLine.size()<<endl;
//    newLine.draw();
    
    if(bSendContours){
        const vector<ofPoint> points = newLine.getVertices();
        for(int i=0; i< newLine.size(); i++){
            m.addFloatArg(points[i].x);
            m.addFloatArg(points[i].y);
        }
    }
    sender.sendMessage(m);
}

	vector<cv::Point2f> toCv(const ofPolyline& polyline) {
		vector<cv::Point2f> contour(polyline.size());
		for(int i = 0; i < polyline.size(); i++) {
			contour[i].x = polyline[i].x;
			contour[i].y = polyline[i].y;
		}
		return contour;		
	}

void phdGimbal2d::getKeyPoints(ofPolyline & _keys) {
	_keys.clear();
	_keys.addVertex( 1.00, 0.0);
	_keys.addVertex( 0.75, 0.0);
	_keys.addVertex( 0.00, 0.0);
	_keys.addVertex( 0.00,-1.0);
	getTransformedPolyline(_keys, _keys, getOTSMatrix());
}

void LaserManager::addLaserPolyline(const ofPolyline& line, ColourSystem* coloursystem, float intens){
	
	if((line.getVertices().size()==0)||(line.getPerimeter()<0.1)) return;
	
	shapes.push_back(new LaserPolyline(line, coloursystem, intens));
	
	
}

void ofCairoRenderer::draw(ofPolyline & poly){
	cairo_new_path(cr);
	for(int i=0;i<(int)poly.size();i++){
		cairo_line_to(cr,poly.getVertices()[i].x,poly.getVertices()[i].y);
	}
	if(poly.isClosed())
		cairo_close_path(cr);
	cairo_stroke( cr );
}

// Backported from oF-dev branch on github
float ShapeUtils::polylineArea(const ofPolyline &poly) {
    if (poly.size() < 2) return 0;

    float area = 0;
    for (int i = 0; i < (int) poly.size() - 1; i++) {
        area += poly[i].x * poly[i+1].y - poly[i+1].x * poly[i].y;
    }
    area += poly[poly.size()-1].x * poly[0].y - poly[0].x * poly[poly.size()-1].y;
    return 0.5 * area;
}

void AnimatedShadow::insertHole( ofPolyline &holeContourLine ){
    if (shapes.size() > 0){
        holeContourLine.setClosed(true);    
        holeContourLine.simplify(1);
        
        int lastFrame = shapes.size()-1;
        shapes[lastFrame].hole = holeContourLine.getSmoothed(1,1);
        shapes[lastFrame].haveHole = true;
    }
}

//----------------------------------------------------------
void ofGLRenderer::draw(ofPolyline & poly){
	// use smoothness, if requested:
	if (bSmoothHinted) startSmoothing();

	glEnableClientState(GL_VERTEX_ARRAY);
	glVertexPointer(3, GL_FLOAT, sizeof(ofVec3f), &poly.getVertices()[0].x);
	glDrawArrays(poly.isClosed()?GL_LINE_LOOP:GL_LINE_STRIP, 0, poly.size());

	// use smoothness, if requested:
	if (bSmoothHinted) endSmoothing();
}

bool operator==(const ofPolyline& lhs, const ofPolyline& rhs)
{
    vector<ofPoint> vertices1 = lhs.getVertices();
    vector<ofPoint> vertices2 = rhs.getVertices();
    if(vertices1.size() != vertices2.size()) return false;
    else {
        for(int i = 0; i < vertices1.size(); i++) {
            if(vertices1[i] != vertices2[i]) return false;
        }
    }
    return true;
}

void ropeMesh::draw(ofPolyline stroke){

    if (stroke.hasChanged()) {
        
        mesh.clear();
        
        mesh.setMode(OF_PRIMITIVE_TRIANGLE_STRIP);
        
        vector < ofPoint > pts = stroke.getVertices();
        
        for (int i = 0; i < pts.size(); i++){
            
            int i_m_1 = MAX(i-1,0);
            int i_p_1 = MIN(i+1, pts.size()-1);
            
            ofPoint pta = pts[i_m_1];
            ofPoint ptb = pts[i];
            ofPoint ptc = pts[i_p_1];
            
            ofPoint diff = ptc - pta;
            
            float angle = atan2(diff.y, diff.x);
            
            angle += PI/2;
            
            float width = 3; //diff.length();
            
            ofPoint offsetA;
            offsetA.x = ptb.x + width * cos(angle);
            offsetA.y = ptb.y + width * sin(angle);
            
            ofPoint offsetB;
            offsetB.x = ptb.x - width * cos(angle);
            offsetB.y = ptb.y - width * sin(angle);
            
            ofSetColor(123,94,65);
          
            ofLine(offsetA, offsetB);
            
            mesh.addVertex(offsetA);
            mesh.addVertex(offsetB);
        }
    
        ofSetColor(197,155,108);
        ofFill();
        mesh.draw();
        ofSetRectMode(OF_RECTMODE_CENTER);
        if (stroke.size()>0) {
             top[num].draw(stroke.getVertices()[stroke.size()-1], width, height);
        }
    }
    
}

void bezierSplinePoints(ofPolyline pnts, int count, int segments, ofPolyline & points) {
    double mu, mudelta;
    int x1, y1, x2, y2, n, h;
    ofVec2f pha, phb;

    if(count < 4 || count > 16383) return;

    // Phantom Points
    pha = ofVec2f(2.0*pnts[0].x-pnts[1].x, 2.0*pnts[0].y-pnts[1].y);
    phb = ofVec2f(2.0*pnts[count-1].x-pnts[count-2].x, 2.0*pnts[count-1].y-pnts[count-2].y);

    mudelta = 1.0 / segments;

    for(n = 2; n < count; n++) {

        mu = 0;

        if(n == 2) {
            x1 = Calculate(mu,pha.x,pnts[n-2].x,pnts[n-1].x,pnts[n].x);
            y1 = Calculate(mu,pha.y,pnts[n-2].y,pnts[n-1].y,pnts[n].y);
        } else if(n == count) {
            x1 = Calculate(mu,pnts[n-3].x,pnts[n-2].x,pnts[n-1].x,phb.x);
            y1 = Calculate(mu,pnts[n-3].y,pnts[n-2].y,pnts[n-1].y,phb.y);
        } else {
            x1 = Calculate(mu,pnts[n-3].x,pnts[n-2].x,pnts[n-1].x,pnts[n].x);
            y1 = Calculate(mu,pnts[n-3].y,pnts[n-2].y,pnts[n-1].y,pnts[n].y);
        }

        points.addVertex(x1, y1);

        mu = mu + mudelta;

        for(h = 1; h < segments; h++) {

            if(n == 2) {
                x2 = Calculate(mu,pha.x,pnts[n-2].x,pnts[n-1].x,pnts[n].x);
                y2 = Calculate(mu,pha.y,pnts[n-2].y,pnts[n-1].y,pnts[n].y);
            } else if(n == count) {
                x2 = Calculate(mu,pnts[n-3].x,pnts[n-2].x,pnts[n-1].x,phb.x);
                y2 = Calculate(mu,pnts[n-3].y,pnts[n-2].y,pnts[n-1].y,phb.y);
            } else {
                x2 = Calculate(mu,pnts[n-3].x,pnts[n-2].x,pnts[n-1].x,pnts[n].x);
                y2 = Calculate(mu,pnts[n-3].y,pnts[n-2].y,pnts[n-1].y,pnts[n].y);
            }

            points.addVertex(x2, y2);

            mu = mu + mudelta;
        }
    }
}

// Backported from oF-dev branch on github
ofPoint ShapeUtils::getCentroid2D(const ofPolyline &poly) {
    ofPoint centroid;
    for(int i=0;i<(int)poly.size()-1;i++){
        centroid.x += (poly[i].x + poly[i+1].x) * (poly[i].x*poly[i+1].y - poly[i+1].x*poly[i].y);
        centroid.y += (poly[i].y + poly[i+1].y) * (poly[i].x*poly[i+1].y - poly[i+1].x*poly[i].y);
    }
    centroid.x += (poly[poly.size()-1].x + poly[0].x) * (poly[poly.size()-1].x*poly[0].y - poly[0].x*poly[poly.size()-1].y);
    centroid.y += (poly[poly.size()-1].y + poly[0].y) * (poly[poly.size()-1].x*poly[0].y - poly[0].x*poly[poly.size()-1].y);

    float area = ShapeUtils::polylineArea(poly);
    centroid.x /= (6*area);
    centroid.y /= (6*area);
    return centroid;
}

//--------------------------------------------------------------
void ofxFatLine::setFromPolyline(ofPolyline & poly){
//	ofxFatLine();
	setGlobalColor(ofGetStyle().color);
	setGlobalWidth(ofGetStyle().lineWidth);
	if (!poly.getVertices().empty()){
		addVertices(poly.getVertices());
	for (int i = 0; i <getVertices().size(); i++) {
		addColor(globalColor);
		addWeight(globalWidth);
	}
	update();
	//*/
	}		
}