/* 
* PROJECT: NyARToolkitCPP
* --------------------------------------------------------------------------------
*
* The NyARToolkitCPP is C++ version NyARToolkit class library.
* Copyright (C)2008-2009 Ryo Iizuka
*
* 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/>.
* 
* For further information please contact.
*	http://nyatla.jp/nyatoolkit/
*	<airmail(at)ebony.plala.or.jp> or <nyatla(at)nyatla.jp>
* 
*/

#include "SingleNyIdMarkerProcessor.h"
namespace NyARToolkitCPP
{

	//filterは自分で決める
	SingleNyIdMarkerProcesser::SingleNyIdMarkerProcesser()
	{
		this->_initialized=false;
	}
	SingleNyIdMarkerProcesser::~SingleNyIdMarkerProcesser()
	{
		NyAR_SAFE_DELETE(this->_id_pickup);
		NyAR_SAFE_DELETE(this->_square_list);

		if(this->_initialized==true){
			NyAR_SAFE_DELETE(this->_threshold_detect);
			NyAR_SAFE_DELETE(this->_tobin_filter);
			NyAR_SAFE_DELETE(this->_data_current);
			NyAR_SAFE_DELETE(this->_data_temp);
			NyAR_SAFE_DELETE(this->_bin_raster);
			NyAR_SAFE_DELETE(this->_transmat);
			NyAR_SAFE_DELETE(this->_square_detect);
			this->_initialized=false;
		}
		return;
	}

	void SingleNyIdMarkerProcesser::initInstance(const NyARParam* i_param,const INyIdMarkerDataEncoder* i_encoder,int i_raster_format)
	{
		NyAR_ASSERT(this->_initialized==false);
		const TNyARIntSize &scr_size = i_param->getScreenSize();
		this->_id_pickup=new NyIdMarkerPickup();
		// 解析オブジェクトを作る
		this->_square_detect = new NyARSquareDetector_Rle(i_param->getDistortionFactor(), scr_size);
		this->_transmat = new NyARTransMat(i_param);
		this->_encoder=i_encoder;

		// ２値画像バッファを作る
		this->_bin_raster = new NyARBinRaster(scr_size.w, scr_size.h);
		//ワーク用のデータオブジェクトを２個作る
		this->_is_active=false;
		this->_data_temp=i_encoder->createDataInstance();
		this->_data_current=i_encoder->createDataInstance();
		this->_tobin_filter = new NyARRasterFilter_ARToolkitThreshold(110,i_raster_format);
		this->_threshold_detect=new NyARRasterThresholdAnalyzer_SlidePTile(15,i_raster_format,4);
		this->_square_list=new NyARSquareStack(100);
		this->_marker_width=100.0;
		this->_current_threshold=110;
		this->_lost_delay_count=0;
		this->_lost_delay = 5;
		this->_initialized=true;
		return;
	}


	void SingleNyIdMarkerProcesser::setMarkerWidth(int i_width)
	{
		this->_marker_width=i_width;
		return;
	}

	void SingleNyIdMarkerProcesser::reset(bool i_is_force)
	{
		if (this->_data_current!=NULL && i_is_force == false) {
			// 強制書き換えでなければイベントコール
			this->onLeaveHandler();
		}
		// カレントマーカをリセット
		this->_data_current = NULL;
		return;
	}

	void SingleNyIdMarkerProcesser::detectMarker(const INyARRgbRaster &i_raster)
	{
		// サイズチェック
		if (!this->_bin_raster->getSize().isEqualSize(i_raster.getSize())) {
			throw NyARException();
		}
		// ラスタを２値イメージに変換する.
		this->_tobin_filter->setThreshold(this->_current_threshold);
		this->_tobin_filter->doFilter(i_raster,*this->_bin_raster);

		NyARSquareStack &square_stack = *this->_square_list;
		// スクエアコードを探す
		this->_square_detect->detectMarker(*this->_bin_raster, square_stack);
		// 認識処理
		if (!this->_is_active) {
			// マーカ未認識→新規認識
			detectNewMarker(i_raster, square_stack);
		} else {
			// マーカ認識依頼→継続認識
			detectExistMarker(i_raster, square_stack);
		}
		return;
	}



	/**新規マーカ検索 現在認識中のマーカがないものとして、最も認識しやすいマーカを１個認識します。
	*/
	void SingleNyIdMarkerProcesser::detectNewMarker(const INyARRgbRaster& i_raster, NyARSquareStack& i_stack)
	{
		TNyIdMarkerParam param;
		TNyIdMarkerPattern patt_data;
		int number_of_square = i_stack.getLength();
		const NyARSquare* current_square=NULL;
		const INyIdMarkerData* marker_id=NULL;
		for (int i = 0; i < number_of_square; i++) {
			// 評価基準になるパターンをイメージから切り出す
			current_square=(const NyARSquare*) i_stack.getItem(i);
			if (!this->_id_pickup->pickFromRaster(i_raster,*current_square,patt_data,param)) {
				continue;
			}
			//エンコード
			if(!this->_encoder->encode(patt_data,*this->_data_temp)){
				continue;
			}
			//認識率が一番高いもの（占有面積が一番大きいもの）を選択する(省略)
			//id認識が成功したら終了
			marker_id=this->_data_temp;
			break;
		}

		// 認識状態を更新
		const bool is_id_found=updateStatus(*current_square,marker_id, param);

		//閾値フィードバック(detectExistMarkerにもあるよ)
		if(is_id_found){
			//マーカがあれば、マーカの周辺閾値を反映
			this->_current_threshold=(this->_current_threshold+param.threshold)/2;
		}else{
			//マーカがなければ、探索+DualPTailで基準輝度検索
			this->_threshold_detect->analyzeRaster(i_raster);
			this->_current_threshold=(this->_current_threshold+this->_threshold_detect->getThreshold())/2;
		}
		return;
	}

	/**マーカの継続認識 現在認識中のマーカを優先して認識します。 
	* （注）この機能はたぶん今後いろいろ発展するからNewと混ぜないこと。
	*/

	void SingleNyIdMarkerProcesser::detectExistMarker(const INyARRgbRaster &i_raster, NyARSquareStack &i_stack)
	{
		TNyIdMarkerParam param;
		TNyIdMarkerPattern patt_data;
		int number_of_square = i_stack.getLength();
		const NyARSquare* current_square=NULL;
		const INyIdMarkerData* marker_id=NULL;
		for (int i = 0; i < number_of_square; i++){
			//idマーカを認識
			current_square=(const NyARSquare*) i_stack.getItem(i);
			if (!this->_id_pickup->pickFromRaster(i_raster,*current_square, patt_data, param)) {
				continue;
			}
			if(!this->_encoder->encode(patt_data,*this->_data_temp)){
				continue;
			}
			//現在認識中のidか確認
			if(!this->_data_current->isEqual((*this->_data_temp))){
				continue;
			}
			//現在認識中のものであれば、終了
			marker_id=this->_data_temp;
			break;
		}
		// 認識状態を更新
		const bool is_id_found=updateStatus(*current_square,marker_id,param);

		//閾値フィードバック(detectExistMarkerにもあるよ)
		if(is_id_found){
			//マーカがあれば、マーカの周辺閾値を反映
			this->_current_threshold=(this->_current_threshold+param.threshold)/2;
		}else{
			//マーカがなければ、探索+DualPTailで基準輝度検索
			this->_threshold_detect->analyzeRaster(i_raster);
			this->_current_threshold=(this->_current_threshold+this->_threshold_detect->getThreshold())/2;
		}
		return;
	}

	bool SingleNyIdMarkerProcesser::updateStatus(const NyARSquare &i_square,const INyIdMarkerData* i_marker_data,const TNyIdMarkerParam &i_param)
	{
		bool is_id_found=false;
		NyARTransMatResult& result=this->__NyARSquare_result;
		//= this.__NyARSquare_result;
		if (!this->_is_active) {// 未認識中
			if (i_marker_data==NULL) {// 未認識から未認識の遷移
				// なにもしないよーん。
				this->_is_active=false;
			} else {// 未認識から認識の遷移
				this->_data_current->copyFrom(*i_marker_data);
				// イベント生成
				// OnEnter
				this->onEnterHandler(*this->_data_current);
				// 変換行列を作成
				this->_transmat->transMat(i_square,i_param.direction, this->_marker_width, result);
				// OnUpdate
				this->onUpdateHandler(i_square, result);
				this->_lost_delay_count = 0;
				this->_is_active=true;
				is_id_found=true;
			}
		} else {// 認識中
			if (i_marker_data==NULL) {
				// 認識から未認識の遷移
				this->_lost_delay_count++;
				if (this->_lost_delay < this->_lost_delay_count) {
					// OnLeave
					this->onLeaveHandler();
					this->_is_active=false;
				}
			} else if(this->_data_current->isEqual(*i_marker_data)) {
				//同じidの再認識
				this->_transmat->transMat(i_square, i_param.direction, this->_marker_width, result);
				// OnUpdate
				this->onUpdateHandler(i_square, result);
				this->_lost_delay_count = 0;
				is_id_found=true;
			} else {// 異なるコードの認識→今はサポートしない。
				throw NyARException();
			}
		}
		return is_id_found;
	}	

}
