/////////////////////////////////////////////////////////////////////////////
// File:        mod_vision.cpp
// Author:      Cesar Mauri Loba (cesar at crea-si dot com)
// Copyright:   (C) 2011 Cesar Mauri Loba - CREA Software Systems
//
//  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 "spcore/component.h"
#include "spcore/module.h"
#include "spcore/basictypes.h"
#include "spcore/libimpexp.h"
#include "spcore/pinimpl.h"
#include "mod_camera/iplimagetype.h"
#include "oftracker.h"

#include <string>
#include <boost/thread/mutex.hpp>
#include <time.h>


using namespace spcore;
using namespace mod_camera;

namespace mod_vision {


enum {
	MAX_INTERFRAME_TIME= 2
};

/* ******************************************************************************
	optical_flow_tracker component
****************************************************************************** */
class OpticalFlowTracker : public spcore::CComponentAdapter {
public:
	static const char* getTypeName() { return "optical_flow_tracker"; };
	virtual const char* GetTypeName() const { return OpticalFlowTracker::getTypeName(); };

	OpticalFlowTracker(const char * name, int argc, const char * argv[])
	: CComponentAdapter(name, argc, argv)
	, m_lastSeen(0)
	{
		m_oPinResult= CTypeComposite::CreateOutputPin("motion");
		if (m_oPinResult.get()== NULL)
			// Output pin creation failed. Shouldn't happen but...
			throw std::runtime_error("optical_flow_tracker. output pin creation failed.");
		RegisterOutputPin (*m_oPinResult);
		RegisterInputPin (*SmartPtr<InputPinImage>(new InputPinImage(*this), false));
		RegisterInputPin (*SmartPtr<InputPinROI>(new InputPinROI(*this), false));

		m_result= CTypeComposite::CreateInstance();
		m_rx= CTypeFloat::CreateInstance();
		m_ry= CTypeFloat::CreateInstance();
		m_result->AddChild (m_rx);
		m_result->AddChild (m_ry);
	}

	void OnImage(const CTypeIplImage & img)
	{
		float vx= 0.0, vy= 0.0;
		m_mutex.lock();
		m_tracker.ProcessImage(*img.getImage(), vx, vy);
		m_mutex.unlock();

		time_t now= time(NULL);

		if (now - m_lastSeen< MAX_INTERFRAME_TIME) {
			// If too many time passed since last frame don't send output
			m_rx->setValue(vx);
			m_ry->setValue(vy);

			m_oPinResult->Send(m_result);
		}
		m_lastSeen= now;
	}

	void UpdateRootROI (const CTypeROI & roi)
	{
		boost::mutex::scoped_lock lock(m_mutex);

		m_tracker.SetROI(roi);
	}

private:
	//
	// Attributes
	//

	// Output pin is a composite of 2 floats with the motion direction
	SmartPtr<IOutputPin> m_oPinResult;
	COfTracker m_tracker;
	boost::mutex m_mutex;
	time_t m_lastSeen;
	SmartPtr<CTypeComposite> m_result;
	SmartPtr<CTypeFloat> m_rx;
	SmartPtr<CTypeFloat> m_ry;


	// Write-only pin to send the image
	class InputPinImage : public spcore::CInputPinWriteOnly<CTypeIplImage, OpticalFlowTracker> {
	public:
		InputPinImage (OpticalFlowTracker & component)
		: CInputPinWriteOnly<CTypeIplImage, OpticalFlowTracker>("image", component) { }

		virtual int DoSend(const CTypeIplImage & img) {
			m_component->OnImage(img);
			return 0;
		}
	};

	// Write-only pin to send a ROI
	class InputPinROI : public spcore::CInputPinWriteOnly<CTypeROI, OpticalFlowTracker> {
	public:
		InputPinROI (OpticalFlowTracker & component)
		: CInputPinWriteOnly<CTypeROI, OpticalFlowTracker>("roi", component) { }

		virtual int DoSend(const CTypeROI & roi) {
			m_component->UpdateRootROI (roi);
			return 0;
		}
	};
};

typedef ComponentFactory<OpticalFlowTracker> OpticalFlowTrackerFactory;

/* ******************************************************************************
	 vision  module
****************************************************************************** */
class VisionModule : public CModuleAdapter {
public:
	VisionModule() {
		//
		// components
		//
		RegisterComponentFactory(SmartPtr<spcore::IComponentFactory>(new OpticalFlowTrackerFactory(), false));
	}
	virtual const char * GetName() const { return "mod_vision"; }
};

static VisionModule* g_module= NULL;
SPEXPORT_FUNCTION spcore::IModule* module_create_instance()
{
	if (g_module== NULL) g_module= new VisionModule();
	return g_module;
}

};
