/* 
* 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 "SingleARMarkerProcessor.h"
#include "nyarcore.h"
namespace NyARToolkitCPP
{
	/**
	* このクラスは、同時に１個のマーカを処理することのできる、アプリケーションプロセッサです。
	* マーカの出現・移動・消滅を、イベントで通知することができます。
	* クラスには複数のマーカを登録できます。一つのマーカが見つかると、プロセッサは継続して同じマーカを
	* １つだけ認識し続け、見失うまでの間は他のマーカを認識しません。
	* 
	* イベントは、 OnEnter→OnUpdate[n]→OnLeaveの順で発生します。
	* マーカが見つかるとまずOnEnterが１度発生して、何番のマーカが発見されたかがわかります。
	* 次にOnUpdateにより、現在の変換行列が連続して渡されます。最後にマーカを見失うと、OnLeave
	* イベントが発生します。
	* 
	*/
	SingleARMarkerProcesser::SingleARMarkerProcesser()
	{
		this->_cf_threshold_new = 0.30;
		this->_cf_threshold_exist = 0.15;
		this->_threshold = 110;
		this->_current_arcode_index = -1;
		this->_lost_delay_count = 0;
		this->_lost_delay = 5;
		this->_square_list = new NyARSquareStack(100);
		this->_initialized=false;
		return;
	}
	SingleARMarkerProcesser::~SingleARMarkerProcesser()
	{
		//setARCodeTableで割り当てられたオブジェクトを開放
		freeARCodeTable();
		NyAR_SAFE_DELETE(this->_square_list);
		if(this->_initialized==true){
			//initInstanceで割り当てたオブジェクトを開放
			NyAR_SAFE_DELETE(this->_square_detect);
			NyAR_SAFE_DELETE(this->_transmat);
			NyAR_SAFE_DELETE(this->_bin_raster);
			NyAR_SAFE_DELETE(this->_tobin_filter);
			NyAR_SAFE_DELETE(this->_threshold_detect);
		}
		return;
	}
	void SingleARMarkerProcesser::initInstance(const NyARParam* i_param,int i_raster_type)
	{
		const TNyARIntSize& scr_size = i_param->getScreenSize();
		// 解析オブジェクトを作る
		this->_square_detect = new NyARSquareDetector_Rle(i_param->getDistortionFactor(), scr_size);
		this->_transmat = new NyARTransMat(i_param);
		this->_tobin_filter=new NyARRasterFilter_ARToolkitThreshold(110,i_raster_type);

		// ２値画像バッファを作る
		this->_bin_raster = new NyARBinRaster(scr_size.w, scr_size.h);
		this->_threshold_detect=new NyARRasterThresholdAnalyzer_SlidePTile(15,i_raster_type,4);
		//初期化
		this->_patt=NULL;
		this->_deviation_data=NULL;
		this->_match_patt=NULL;
		this->_initialized=true;
		return;
	}

	void SingleARMarkerProcesser::freeARCodeTable()
	{
		//割当済のオブジェクトを削除
		NyAR_SAFE_DELETE(this->_patt);
		NyAR_SAFE_DELETE(this->_deviation_data);
		if(this->_match_patt!=NULL){
			for(int i=0;i<this->_match_patt->length;i++){
				NyAR_SAFE_DELETE(this->_match_patt->item[i]);
			}
			NyAR_SAFE_DELETE(this->_match_patt);
		}
		return;
	}

	/*自動・手動の設定が出来ないので、コメントアウト
	public void setThreshold(int i_threshold)
	{
	this._threshold = i_threshold;
	return;
	}*/

	/**検出するマーカコードの配列を指定します。 検出状態でこの関数を実行すると、
	* オブジェクト状態に強制リセットがかかります。
	*/

	void SingleARMarkerProcesser::setARCodeTable(NyARCode* const i_ref_code_table[],int i_number_of_code, int i_code_resolution, double i_marker_width)
	{
		if (this->_current_arcode_index != -1) {
			// 強制リセット
			reset(true);
		}
		//割当済のオブジェクトを削除
		freeARCodeTable();

		//検出するマーカセット、情報、検出器を作り直す。(1ピクセル4ポイントサンプリング,マーカのパターン領域は50%)
		this->_patt = new NyARColorPatt_Perspective_O2(i_code_resolution, i_code_resolution,4,25);
		this->_deviation_data=new NyARMatchPattDeviationColorData(i_code_resolution, i_code_resolution);
		this->_marker_width = i_marker_width;

		this->_match_patt = new NyArray<NyARMatchPatt_Color_WITHOUT_PCA*>(i_number_of_code);
		for(int i=0;i<i_number_of_code;i++){
			this->_match_patt->item[i]=new NyARMatchPatt_Color_WITHOUT_PCA(i_ref_code_table[i]);
		}
		return;
	}

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

	void SingleARMarkerProcesser::detectMarker(const INyARRgbRaster& i_raster)
	{
		// サイズチェック
		NyAR_ASSERT(this->_bin_raster->getSize().isEqualSize(i_raster.getSize().w, i_raster.getSize().h));

		// コードテーブルが無ければここで終わり
		if (this->_match_patt== NULL) {
			return;
		}

		// ラスタを(1/4の画像の)２値イメージに変換する.
		this->_tobin_filter->setThreshold(this->_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->_current_arcode_index == -1) { // マーカ未認識
			detectNewMarker(i_raster, square_stack);
		} else { // マーカ認識中
			detectExistMarker(i_raster, square_stack, this->_current_arcode_index);
		}
		return;
	}


	/**ARCodeのリストから、最も一致するコード番号を検索します。
	*/
	bool SingleARMarkerProcesser::selectARCodeIndexFromList(const INyARRgbRaster& i_raster,const NyARSquare& i_square, TResult_selectARCodeIndex& o_result)
	{
		// 現在コードテーブルはアクティブ？
		if (this->_match_patt==NULL) {
			return false;
		}
		// 評価基準になるパターンをイメージから切り出す
		if (!this->_patt->pickFromRaster(i_raster, i_square)) {
			return false;
		}
		//評価データを作成して、評価器にセット
		this->_deviation_data->setRaster(*this->_patt);		
		TNyARMatchPattResult mr;
		int code_index = 0;
		int dir = 0;
		double c1 = 0;
		// コードと比較する
		for (int i = 0; i < this->_match_patt->length; i++) {
			this->_match_patt->item[i]->evaluate(*this->_deviation_data,mr);
			double c2 = mr.confidence;
			if (c1 < c2) {
				code_index = i;
				c1 = c2;
				dir = mr.direction;
			}
		}
		o_result.code_index = code_index;
		o_result.direction = dir;
		o_result.confidence = c1;
		return true;
	}

	/**新規マーカ検索 現在認識中のマーカがないものとして、最も認識しやすいマーカを１個認識します。
	*/
	void SingleARMarkerProcesser::detectNewMarker(const INyARRgbRaster& i_raster,NyARSquareStack& i_stack)
	{
		int number_of_square = i_stack.getLength();
		double cf = 0;
		int dir = 0;
		int code_index = -1;
		int square_index = 0;
		TResult_selectARCodeIndex detect_result;
		for (int i = 0; i < number_of_square; i++) {
			if (!selectARCodeIndexFromList(i_raster,*i_stack.getItem(i), detect_result)) {
				// 見つからない。
				return;
			}
			if (detect_result.confidence < this->_cf_threshold_new) {
				continue;
			}
			if (detect_result.confidence < cf) {
				// 一致度が低い。
				continue;
			}
			cf = detect_result.confidence;
			code_index = detect_result.code_index;
			square_index = i;
			dir = detect_result.direction;
		}
		// 認識状態を更新
		const bool is_id_found=updateStatus(*this->_square_list->getItem(square_index), code_index, cf, dir);
		//閾値フィードバック(detectExistMarkerにもあるよ)
		if(!is_id_found){
			//マーカがなければ、探索+DualPTailで基準輝度検索
			this->_threshold_detect->analyzeRaster(i_raster);
			this->_threshold=(this->_threshold+this->_threshold_detect->getThreshold())/2;
		}
	}

	/**マーカの継続認識 現在認識中のマーカを優先して認識します。 
	* （注）この機能はたぶん今後いろいろ発展するからNewと混ぜないこと。
	*/
	void SingleARMarkerProcesser::detectExistMarker(const INyARRgbRaster& i_raster,const NyARSquareStack& i_stack, int i_current_id)
	{
		int number_of_square = i_stack.getLength();
		double cf = 0;
		int dir = 0;
		int code_index = -1;
		int square_index = 0;
		TResult_selectARCodeIndex detect_result;
		for (int i = 0; i < number_of_square; i++) {
			if (!selectARCodeIndexFromList(i_raster,*i_stack.getItem(i), detect_result)) {
				// 見つからない。
				return;
			}
			// 現在のマーカを認識したか？
			if (detect_result.code_index != i_current_id) {
				// 認識中のマーカではないので無視
				continue;
			}
			if (detect_result.confidence < this->_cf_threshold_exist) {
				continue;
			}
			if (detect_result.confidence < cf) {
				// 一致度が高い方を選ぶ
				continue;
			}
			cf = detect_result.confidence;
			code_index = detect_result.code_index;
			dir = detect_result.direction;
			square_index = i;
		}
		// 認識状態を更新
		const bool is_id_found=updateStatus(*this->_square_list->getItem(square_index), code_index, cf, dir);
		//閾値フィードバック(detectExistMarkerにもあるよ)
		if(!is_id_found){
			//マーカがなければ、探索+DualPTailで基準輝度検索
			this->_threshold_detect->analyzeRaster(i_raster);
			this->_threshold=(this->_threshold+this->_threshold_detect->getThreshold())/2;
		}

	}

	/**	オブジェクトのステータスを更新し、必要に応じてハンドル関数を駆動します。
	* 	戻り値は、「実際にマーカを発見する事ができたか」です。クラスの状態とは異なります。
	*/
	bool SingleARMarkerProcesser::updateStatus(const NyARSquare& i_square, int i_code_index, double i_cf, int i_dir)
	{
		NyARTransMatResult& result = this->__NyARSquare_result;
		if (this->_current_arcode_index < 0) {// 未認識中
			if (i_code_index < 0) {// 未認識から未認識の遷移
				// なにもしないよーん。
				return false;
			} else {// 未認識から認識の遷移
				this->_current_arcode_index = i_code_index;
				// イベント生成
				// OnEnter
				this->onEnterHandler(i_code_index);
				// 変換行列を作成
				this->_transmat->transMat(i_square, i_dir, this->_marker_width, result);
				// OnUpdate
				this->onUpdateHandler(i_square, result);
				this->_lost_delay_count = 0;
				return true;
			}
		} else {// 認識中
			if (i_code_index < 0) {// 認識から未認識の遷移
				this->_lost_delay_count++;
				if (this->_lost_delay < this->_lost_delay_count) {
					// OnLeave
					this->_current_arcode_index = -1;
					this->onLeaveHandler();
				}
				return false;
			} else if (i_code_index == this->_current_arcode_index) {// 同じARCodeの再認識
				// イベント生成
				// 変換行列を作成
				this->_transmat->transMat(i_square, i_dir, this->_marker_width, result);
				// OnUpdate
				this->onUpdateHandler(i_square, result);
				this->_lost_delay_count = 0;
				return true;
			} else {// 異なるコードの認識→今はサポートしない。
				throw NyARException();
			}
		}
	}
}
