
#include "contourAnalyser.h"

//---------------------------------------------
contourAnalyser::contourAnalyser(){
}
	
	
//---------------------------------------------
void contourAnalyser::smoothContour(ofxCvBlob blob, vector <ofxVec2f> & smoothedPts,  float smoothAmnt){
	
	smoothedPts.clear();
	smoothedPts.assign(blob.nPts, ofxVec2f());
	
	smoothedPts[0].x = blob.pts[0].x;
	smoothedPts[0].y = blob.pts[0].y;
	
	for (int j = 1; j < blob.nPts; j++){
		smoothedPts[j].x = blob.pts[j].x;
		smoothedPts[j].y = blob.pts[j].y;

		smoothedPts[j].x = smoothedPts[j].x * (1.0 - smoothAmnt)  + smoothedPts[j-1].x * smoothAmnt;
		smoothedPts[j].y = smoothedPts[j].y * (1.0 - smoothAmnt)  + smoothedPts[j-1].y * smoothAmnt;
	}
	
}
	


//---------------------------------------------
void contourAnalyser::resampleContour(vector <ofxVec2f> &contour, int resampleAmount){

	
	ofxVec2f	  lower;
	ofxVec2f	  upper;	
	
	vector <ofxVec2f> output;

	int 		 nResampledPoints = resampleAmount;
	int 		 nPoints = contour.size();
	int 	     nPathPoints = nPoints;
	float	     totalPathLength = 0;
 	
	for (int i = 0; i < contour.size()-1; i++){
		float dx = contour[i].x -  contour[i+1].x;
		float dy = contour[i].y -  contour[i+1].y;
		totalPathLength += sqrt(dx*dx + dy*dy);
	}
	
	float	     newPathLength = 0;
	float	     RSL = totalPathLength / (float)(nResampledPoints-1); // resampledSegmentLength
	float	     Dx, Dy, Dz;
	float	     dx, dy, dz;
	float	     px, py, pz; 		
	float	     RSLdx, RSLdy, RSLdz;
	
	float	     segLength, ASL; // available segment length
	float	     remainder, neededSpace;
	float	     prevRemainder = RSL;
	int 	     nsegs;
	int 	     p=0;
	int 	     i;
	int			 nPtsToDo; 
	int			 nResamples = 0;
	
	/* TODO : add this case. this is a special case...
	 
	 if (nPathPoints <= 1) { // special case for one-point path
		for (p=0; p<nResampledPoints; p++){
			lower = in[0]; 				
			px = lower.x + (float)p*0.0001;
			py = lower.y + (float)p*0.0001; 				
			pz = lower.z;
			out[nResamples]->x = px;
			out[nResamples]->y = py;
			out[nResamples]->z = pz;
			nResamples++;
		} 	
		
	} else { 			*/
	
	
	for (i=0; i<nPathPoints; i++){
			
			lower = contour[i];
			upper = contour[(i+1)%nPathPoints];
			Dx = upper.x - lower.x;
			Dy = upper.y - lower.y; 				
			Dz = upper.z - lower.z;
			
			if (Dx == 0 && Dy == 0){ 
				Dx = 0.0001f; Dy = 0.0001f; 
			}

			segLength = sqrtf(Dx*Dx + Dy*Dy);
			ASL = segLength;		// available segment length 				
			dx = (Dx/segLength);	// unit vector components
			dy = (Dy/segLength); 				
			dz = (Dz/segLength);
			
			RSLdx = dx * RSL;		// resampled segment vector components
			RSLdy = dy * RSL;
			RSLdz = dz * RSL; 
			
			neededSpace = RSL-prevRemainder;
			
			if (ASL >= neededSpace){ 
				
				// if there is enough room to place the first point
				// then place the first resample point in the latest segment
				remainder = ASL;
				px = lower.x + (neededSpace*dx);
				py = lower.y + (neededSpace*dy); 					
				pz = upper.z + (neededSpace*dz);
				
				if (p < (nResampledPoints-1)){ 
					output.push_back(ofxVec2f(px, py));
					nResamples++;
					newPathLength += RSL;
					remainder -= neededSpace;
					p++;
				}
				
				nPtsToDo = (int)(remainder/RSL);
				for (int d=0; d<nPtsToDo; d++){
					px += RSLdx;  						
					py += RSLdy; 						
					pz += RSLdz; 						
					if (p < (nResampledPoints-1)){ 
						output.push_back(ofxVec2f(px, py));
						//out[nResamples]->x = px;
						//out[nResamples]->y = py;
						//out[nResamples]->z = pz;
						nResamples++;	
						newPathLength += RSL;
						remainder -= RSL;
						p++;
					}
				}
				prevRemainder = remainder;
			} else { 
				// if there is not enough room to place the first point 
				prevRemainder += ASL; 				
			} 			
		}
		upper = contour[nPoints-1]; 	
		output.push_back(ofxVec2f(upper.x, upper.y));
		nResamples++;	
	
		// now get that output vector into the input: 
		contour.clear();
		for (int i = 0; i < output.size(); i++){
			contour.push_back(ofxVec2f( output[i].x, output[i].y));
		}
		output.clear();
		
}
