/////////////////////////////////////////////////////////////////////////////
// Copyright:   (C) 2008 Cesar Mauri (cesar at crea-si dot com)
// 
//  This program 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 3 of the License, or
//  (at your option) any later version.
//
//  This program 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 this program.  If not, see <http://www.gnu.org/licenses/>.
/////////////////////////////////////////////////////////////////////////////
#include "oftracker.h"
//#include "crvimage.h"
#include "creavision/crvcolor.h"
//#include "crvdraw.h"

using namespace mod_camera;
using namespace spcore;

namespace mod_vision {

COfTracker::COfTracker() //CCamWindow2* pCamWindow) 
//: m_bigArea (0.9f, 0.9f)
//, m_smallArea (0.2f, 0.2f)
//, m_sensitivy (1.0f)
//, m_autoTrack (false)
//, m_pCamWindow (pCamWindow)
{
//	m_smallArea.SetShowOrientation (true);
//	m_bigArea.getInstance()->RegisterChildROI (&m_smallArea);
	m_roi= CTypeROI::CreateInstance();
}

COfTracker::~COfTracker(void)
{
//	m_bigArea.getInstance()->UnregisterChildROI (&m_smallArea);
}

// TODO: manage allocation errors
bool COfTracker::AllocateImages(const IplImage& image)
{
	bool changed= false;

    // Current/Previous
	if (!m_currentImg.Initialized() || 
		m_currentImg.Width()!= image.width || m_currentImg.Height()!= image.height)
	{
		m_currentImg.Free();
		m_currentImg.Create (image.width, image.height, IPL_DEPTH_8U, "GRAY", IPL_ORIGIN_TL, IPL_ALIGN_DWORD);

		m_previousImg.Free();
		m_previousImg.Create (image.width, image.height, IPL_DEPTH_8U, "GRAY", IPL_ORIGIN_TL, IPL_ALIGN_DWORD);

		m_diffImg.Free();
		m_diffImg.Create (image.width, image.height, IPL_DEPTH_8U, "GRAY", IPL_ORIGIN_TL, IPL_ALIGN_DWORD);

		changed= true;
	}

	if (!m_velX.Initialized() || m_velX.Width()!= image.width || m_velX.Height()!= image.height)
	{
		m_velX.Free();
		m_velX.Create (image.width, image.height, IPL_DEPTH_32F, "GRAY", IPL_ORIGIN_TL, IPL_ALIGN_DWORD);
	
		m_velY.Free();
		m_velY.Create (image.width, image.height, IPL_DEPTH_32F, "GRAY", IPL_ORIGIN_TL, IPL_ALIGN_DWORD);
	
		changed= true;
	}

	return changed;
}

int COfTracker::ProcessImage(const IplImage& image, float &x, float &y)
{
	if (AllocateImages(image))
	{
		// Changed
		crvColorToGray (&image, m_previousImg.ptr());
		x= y= 0;
	}
	else
	{
		//cvSplit( pImage, NULL, NULL, currentImg.ptr(), NULL);
		crvColorToGray (&image, m_currentImg.ptr());

#if 0
		//
		// Locate motion and position box
		//
		if (GetAutoTrack())
		{
			double m0;
			CvMoments moments;

			int roiX, roiY, roiWidth, roiHeight;
			m_bigArea.getInstance()->GetBoxImg (&m_previousImg, roiX, roiY, roiWidth, roiHeight);

			m_previousImg.PushROI();
			m_previousImg.SetROI (roiX, roiY, roiWidth, roiHeight);

			m_currentImg.PushROI();
			m_currentImg.SetROI (roiX, roiY, roiWidth, roiHeight);

			m_diffImg.PushROI();
			m_diffImg.SetROI (roiX, roiY, roiWidth, roiHeight);

			cvAbsDiff( m_previousImg.ptr(), m_currentImg.ptr(), m_diffImg.ptr() );
			cvThreshold( m_diffImg.ptr(), m_diffImg.ptr(), 20.0, 0.0, CV_THRESH_TOZERO);
			cvMoments ( m_diffImg.ptr(), &moments, 0 );
			m0= cvGetSpatialMoment( &moments, 0, 0 );

			m_diffImg.PopROI();
			m_currentImg.PopROI();
			m_previousImg.PopROI();

			if (m0> 0)
			{
				m_smallArea.SetCenterImg (&m_currentImg, 
										(int) ((cvGetSpatialMoment( &moments, 1, 0 ) / m0) + 0.5f) + roiX,
										(int) ((cvGetSpatialMoment( &moments, 0, 1 ) / m0) + 0.5f) + roiY);
			}
		}
#endif

		//
		// Optical Flow motion analisys
		//

		int roiX= (int) ((float) m_previousImg.Width() * m_roi->GetX());
		int roiY= (int) ((float) m_previousImg.Height() * m_roi->GetY());
		int roiWidth= (int) ((float) m_previousImg.Width() * m_roi->GetWidth());
		int roiHeight= (int) ((float) m_previousImg.Height() * m_roi->GetHeight());

		assert (roiX>= 0 && roiX< m_previousImg.Width());
		assert (roiY>= 0 && roiY< m_previousImg.Height());
		assert (roiWidth>= 0 && roiWidth<= m_previousImg.Width());
		assert (roiHeight>= 0 && roiHeight<= m_previousImg.Height());
//		m_smallArea.GetBoxImg (&m_previousImg, roiX, roiY, roiWidth, roiHeight);
		
		m_previousImg.PushROI();
        m_previousImg.SetROI (roiX, roiY, roiWidth, roiHeight);

		m_currentImg.PushROI();
		m_currentImg.SetROI (roiX, roiY, roiWidth, roiHeight);

		m_velX.SetROI (roiX, roiY, roiWidth, roiHeight);
		m_velY.SetROI (roiX, roiY, roiWidth, roiHeight);

		CvTermCriteria term;
		term.type= CV_TERMCRIT_ITER;
		term.max_iter= 6;
		cvCalcOpticalFlowHS (m_previousImg.ptr(), m_currentImg.ptr(), 0,
							 m_velX.ptr(), m_velY.ptr(), 0.001, term);

		m_currentImg.PopROI();
		m_previousImg.PopROI();
						
		// Compute mean
		cvSmooth( m_velX.ptr(), m_velX.ptr(), CV_GAUSSIAN, 3, 3);               

		CvScalar cs;
		cs= cvSum( m_velX.ptr() );
		x= (float) cs.val[0] / (float) (roiWidth * roiHeight);
		cs= cvSum( m_velY.ptr() );
		y= (float) cs.val[0] / (float) (roiWidth * roiHeight);

		// Apply rotation
		float rotation= atan2 (y,x);
		rotation+= m_roi->GetDirection(); //m_smallArea.GetRotation ();
		float modulus= sqrt (x*x + y*y);
		x= modulus * cos (rotation);
		y= modulus * sin (rotation);
	
		m_currentImg.Swap (&m_previousImg);
	}

	return 1;
}

};