/*
 *  pkmBlobTracker.cpp
 *  Created by Parag K. Mital - http://pkmital.com 
 *  Contact: parag@pkmital.com
 *
 
 Copyright (C) 2011 Parag K. Mital
 
 The Software is and remains the property of Parag K Mital
 ("pkmital") The Licensee will ensure that the Copyright Notice set
 out above appears prominently wherever the Software is used.
 
 The Software is distributed under this Licence: 
 
 - on a non-exclusive basis, 
 
 - solely for non-commercial use in the hope that it will be useful, 
 
 - "AS-IS" and in order for the benefit of its educational and research
 purposes, pkmital makes clear that no condition is made or to be
 implied, nor is any representation or warranty given or to be
 implied, as to (i) the quality, accuracy or reliability of the
 Software; (ii) the suitability of the Software for any particular
 use or for use under any specific conditions; and (iii) whether use
 of the Software will infringe third-party rights.
 
 pkmital disclaims: 
 
 - all responsibility for the use which is made of the Software; and
 
 - any liability for the outcomes arising from using the Software.
 
 The Licensee may make public, results or data obtained from, dependent
 on or arising out of the use of the Software provided that any such
 publication includes a prominent statement identifying the Software as
 the source of the results or the data, including the Copyright Notice
 and stating that the Software has been made available for use by the
 Licensee under licence from pkmital and the Licensee provides a copy of
 any such publication to pkmital.
 
 The Licensee agrees to indemnify pkmital and hold them
 harmless from and against any and all claims, damages and liabilities
 asserted by third parties (including claims for negligence) which
 arise directly or indirectly from the use of the Software or any
 derivative of it or the sale of any products based on the
 Software. The Licensee undertakes to make no liability claim against
 any employee, student, agent or appointee of pkmital, in connection 
 with this Licence or the Software.
 
 
 No part of the Software may be reproduced, modified, transmitted or
 transferred in any form or by any means, electronic or mechanical,
 without the express permission of pkmital. pkmital's permission is not
 required if the said reproduction, modification, transmission or
 transference is done without financial return, the conditions of this
 Licence are imposed upon the receiver of the product, and all original
 and amended source code is included in any transmitted product. You
 may be held legally responsible for any copyright infringement that is
 caused or encouraged by your failure to abide by these terms and
 conditions.
 
 You are not permitted under this Licence to use this Software
 commercially. Use for which any financial return is received shall be
 defined as commercial use, and includes (1) integration of all or part
 of the source code or the Software into a product for sale or license
 by or on behalf of Licensee to third parties or (2) use of the
 Software or any derivative of it for research with the final aim of
 developing software products for sale or license to a third party or
 (3) use of the Software or any derivative of it for research with the
 final aim of developing non-software products for sale or license to a
 third party, or (4) use of the Software to provide any service to an
 external organisation for which payment is received. If you are
 interested in using the Software commercially, please contact pkmital to
 negotiate a licence. Contact details are: parag@pkmital.com
  
 *
 */

#include "pkmBlobTracker.h"

float pkmBlobTracker::getOrientation(ofCvTrackedBlob &blob)
{
	int numPts = blob.contour.size();
	char status[numPts];
	float err[numPts];
	CvPoint2D32f featuresA[numPts];
	CvPoint2D32f featuresB[numPts];
	
	for(int i = 0; i < numPts; i++)
	{
		featuresA[i].x = MAX(MIN(blob.contour[i].x, 317),3);
		featuresA[i].y = MAX(MIN(blob.contour[i].y, 237),3);
	}
	
	cvCalcOpticalFlowPyrLK(cvPrevGrayImage,cvGrayImage,
						   NULL, NULL,
						   &(featuresA[0]),&(featuresB[0]),
						   numPts,cvSize(12,12),levels,status,err,crit,internal_flags);
	
	float ori = 0.0f;
	int pts = 0;
	for (int i = 0; i < numPts; i++) {

		CvPoint2D32f vecA,vecB,vecAB;
		vecA.x = featuresA[i].x;
		vecA.y = featuresA[i].y;
		
		vecB.x = featuresB[i].x;
		vecB.y = featuresB[i].y;
		
		//atan2f((float)vecA.x, (float)vecA.y) - atan2f((float)vecB.x, (float)vecB.y);
		//float normA = sqrtf(vecA.x*vecA.x + vecA.y*vecA.y);
		//float normB = sqrtf(vecB.x*vecB.x + vecB.y*vecB.y);
		//acosf( (vecA.x*vecB.x + vecA.y*vecB.y) / (normA * normB) );
		float o =  fmod(atan2f((float)(vecB.y - vecA.y), (float)(vecB.x - vecA.x)), (float)(2.0f*PI));
				
		if(!isnan(o))
		{
			printf("%f\n", (float)(o*180.0f/PI));
			ori += o;
			pts++;
		}
		
	}
	ori /= pts;
	if (bInit) {
		orientation = fmod((float)(0.6*orientation + 0.4*(orientation + (ori-orientation))), (float)(2.0f*PI));
	}
	else {
		orientation = ori;
		bInit = true;
	}
	center_x = blob.center.x;
	center_y = blob.center.y;
	width = blob.box.width;
	height = blob.box.height;
	
	return orientation;
}
