/*
 * AXMIspAfAlgorithm.cpp
 * AFアルゴリズム演算処理
 *
 */

#include "AXMIspAfAlgorithm.hpp"
#include "AXMImageSensor/imageSensorDrv.h"
#include "AXMIspTest/DSPCommon.h"
#include "AXMIspTest/adrs_base.h"
#include "AXMIspTest/reg_imageif.h"
#include "AXMIspTest/spad_map.h"

#ifndef PARASOFT_CPPTEST
using namespace std;
#endif

#define AF_DEBUG_LOG

#ifdef AF_DEBUG_LOG
#define LOGMAX	1024
#if 0
long st_hEval1_log[LOGMAX];				// 1
long st_hEval2_log[LOGMAX];				// 2
long st_hEval3_log[LOGMAX];				// 3
long st_hEval4_log[LOGMAX];				// 4
long st_hEval5_log[LOGMAX];				// 5
long st_hEval6_log[LOGMAX];				// 6
long st_hEval7_log[LOGMAX];				// 7
long st_hEval8_log[LOGMAX];				// 8
long st_hEval9_log[LOGMAX];				// 9
long st_lEval1_log[LOGMAX];				// 10
long st_lEval2_log[LOGMAX];				// 11
long st_lEval3_log[LOGMAX];				// 12
long st_lEval4_log[LOGMAX];				// 13
long st_lEval5_log[LOGMAX];				// 14
long st_lEval6_log[LOGMAX];				// 15
long st_lEval7_log[LOGMAX];				// 16
long st_lEval8_log[LOGMAX];				// 17
long st_lEval9_log[LOGMAX];				// 18
#endif
long st_lowEval_log[LOGMAX];				// 24
long st_vmax_log[LOGMAX];				// 19
long st_gain_log[LOGMAX];				// 20
long st_shutter_log[LOGMAX];			// 21
long st_AfStatus_log[LOGMAX];			// 22
long st_count_log[LOGMAX];				// 23
long st_Eval_log[LOGMAX];				// 24
long st_path_log[LOGMAX];				// 25
long st_deciValue1_log[LOGMAX];			// 26
long st_deciValue2_log[LOGMAX];			// 27
enum _afLogPathLog_ {
	PATHLOG_StartDirectionJudge_000 = 0x00000001 ,
	PATHLOG_StartDirectionJudge_001 = 0x00000002 ,
	PATHLOG_StartDirectionJudge_002 = 0x00000004 ,
	PATHLOG_StartDirectionJudge_003 = 0x00000008 ,
	PATHLOG_StartDirectionJudge_004 = 0x00000010 ,
	PATHLOG_StartDirectionJudge_005 = 0x00000020 ,
	PATHLOG_StartDirectionJudge_006 = 0x00000040 ,
	PATHLOG_StartDirectionJudge_007 = 0x00000080 ,
	PATHLOG_StartDirectionJudge_008 = 0x00000100 ,
	PATHLOG_StartDirectionJudge_009 = 0x00000200 ,
	PATHLOG_StartDirectionJudge_00A = 0x00000400 ,
	PATHLOG_StartDirectionJudge_00B = 0x00000800 ,
	PATHLOG_StartDirectionJudge_00C = 0x00001000 ,
	PATHLOG_StartDirectionJudge_00D = 0x00002000 ,
	PATHLOG_StartDirectionJudge_00E = 0x00004000 ,
	PATHLOG_StartDirectionJudge_00F = 0x00008000 ,
	PATHLOG_StartDirectionJudge_010 = 0x00010000 ,
	PATHLOG_StartDirectionJudge_011 = 0x00020000 ,
	PATHLOG_StartDirectionJudge_012 = 0x00040000 ,
	PATHLOG_StartDirectionJudge_013 = 0x00080000 ,
	PATHLOG_StartDirectionJudge_014 = 0x00100000 ,
	PATHLOG_StartDirectionJudge_015 = 0x00200000 ,
	PATHLOG_StartDirectionJudge_016 = 0x00400000 ,
	
	PATHLOG_StartDirectionJudgeReturn_000 = 0x00000001 ,
	PATHLOG_StartDirectionJudgeReturn_001 = 0x00000002 ,
	PATHLOG_StartDirectionJudgeReturn_002 = 0x00000004 ,
	PATHLOG_StartDirectionJudgeReturn_003 = 0x00000008 ,
	PATHLOG_StartDirectionJudgeReturn_004 = 0x00000010 ,
	
	PATHLOG_HighSpeedDrive_000      = 0x00000001 ,
	PATHLOG_HighSpeedDrive_001      = 0x00000002 ,
	PATHLOG_HighSpeedDrive_002      = 0x00000004 ,
	PATHLOG_HighSpeedDrive_003      = 0x00000008 ,
	PATHLOG_HighSpeedDrive_004      = 0x00000010 ,
	PATHLOG_HighSpeedDrive_005      = 0x00000020 ,
	PATHLOG_HighSpeedDrive_006      = 0x00000040 ,
	PATHLOG_HighSpeedDrive_007      = 0x00000080 ,
	PATHLOG_HighSpeedDrive_008      = 0x00000100 ,
	PATHLOG_HighSpeedDrive_009      = 0x00000200 ,
	PATHLOG_HighSpeedDrive_00A      = 0x00000400 ,
	PATHLOG_HighSpeedDrive_00B      = 0x00000800 ,
	PATHLOG_HighSpeedDrive_00C      = 0x00001000 ,
	PATHLOG_HighSpeedDrive_00D      = 0x00002000 ,
	PATHLOG_HighSpeedDrive_00E      = 0x00004000 ,
	PATHLOG_HighSpeedDrive_00F      = 0x00008000 ,
	PATHLOG_HighSpeedDrive_010      = 0x00010000 ,
	PATHLOG_HighSpeedDrive_011      = 0x00020000 ,
	PATHLOG_HighSpeedDrive_012      = 0x00040000 ,
	PATHLOG_HighSpeedDrive_013      = 0x00080000 ,
	PATHLOG_HighSpeedDrive_014      = 0x00100000 ,
	PATHLOG_HighSpeedDrive_015      = 0x00200000 ,
	PATHLOG_HighSpeedDrive_016      = 0x00400000 ,
	

	PATHLOG_LowSpeedDrive_000       = 0x00000001 ,
	PATHLOG_LowSpeedDrive_001       = 0x00000002 ,
	PATHLOG_LowSpeedDrive_002       = 0x00000004 ,
	PATHLOG_LowSpeedDrive_003       = 0x00000008 ,
	PATHLOG_LowSpeedDrive_004       = 0x00000010 ,
	PATHLOG_LowSpeedDrive_005       = 0x00000020 ,
	PATHLOG_LowSpeedDrive_006       = 0x00000040 ,

	PATHLOG_PeakDetection_000       = 0x00000001 ,
	PATHLOG_PeakDetection_001       = 0x00000002 ,
	PATHLOG_PeakDetection_002       = 0x00000004 ,
	PATHLOG_PeakDetection_003       = 0x00000008 ,
	PATHLOG_PeakDetection_004       = 0x00000010 ,
	PATHLOG_PeakDetection_005       = 0x00000020 ,
	PATHLOG_PeakDetection_006       = 0x00000040 ,

	PATHLOG_ReturnAndStop_000       = 0x00000001 ,
	PATHLOG_ReturnAndStop_001       = 0x00000002 ,

	PATHLOG_Reboot_000              = 0x00000001 ,
	PATHLOG_Reboot_001              = 0x00000002 ,
	PATHLOG_Reboot_002              = 0x00000004 ,
	PATHLOG_Reboot_003              = 0x00000008 ,
	PATHLOG_Reboot_004              = 0x00000010 ,
	
	PATHLOG_Debug_000               = 0x00000001 ,
	PATHLOG_Debug_001               = 0x00000002 ,
	PATHLOG_Debug_002               = 0x00000004 ,
	PATHLOG_Debug_003               = 0x00000008 ,
	PATHLOG_Debug_004               = 0x00000010 ,
	PATHLOG_Debug_005               = 0x00000020 ,
	
};
#if 0
const long *evalLogTable[18] = {
	st_hEval1_log ,
	st_hEval2_log ,
	st_hEval3_log ,
	st_hEval4_log ,
	st_hEval5_log ,
	st_hEval6_log ,
	st_hEval7_log ,
	st_hEval8_log ,
	st_hEval9_log ,
	st_lEval1_log ,
	st_lEval2_log ,
	st_lEval3_log ,
	st_lEval4_log ,
	st_lEval5_log ,
	st_lEval6_log ,
	st_lEval7_log ,
	st_lEval8_log ,
	st_lEval9_log 
};
#endif
long st_log_index = 0;
#endif

AXMIspAfBase::AXMIspAfBase() {
	// 初動方向、
	// 判定期間（５フレーム）、
	// 判定閾値
	controlAF.lensMove = AF_CONT_STOP;
	status = AF_STS_REBOOT;			// AF状態遷移フラグ

	afStartDir = AF_STADIR_TOMACRO;		// 初動方向
	afStartDirChangeThreshHf = 5000;	//10000;			// 開始時方向切替判定閾値
	afStartDirChangeThreshLf = 5000;	//10000;			// 開始時方向切替判定閾値
	afStartDirChangeThreshRatio = 20;
	afStartDirJudMax = 5;				// 開始時方向切替判定所定回数
	lensAtEndJudg = 200;				// レンズが端にあるか判定用
	afStartDirectJudgCount = 0;			// 開始時方向切替判定カウンタ
	afArea = 4;							// オートフォーカス領域指定0～8
	afOneShotEnable = 1;				// AFワンショットモードOff
	afOneShotStart = 1;					// AFワンショット起動
	for(int i = 0; i < 9 ; i++ ) {
		afLfEval[i] = 0;				// LF評価値
		afHfEval[i] = 0;				// HF評価値
	}
	afHfEval1 = 0;						// 前々回AF評価値
	afHfEval2 = 0;						// 前回AF評価値
	afHfEval3 = 0;						// 今回AF評価値
	afLfEval1 = 0;						// 前々回AF評価値
	afLfEval2 = 0;						// 前回AF評価値
	afLfEval3 = 0;						// 今回AF評価値
	afHfEvalChgStatus = 0;
	detectEndFlag = 0;
	detectChgSpedFlag = 0;
	detectChgDirFlag = 0;
	evalZeroCount = 0;
//	HighSpeedChangeDirThresh = 40000;

	afHfDiffSav = 0;					// 以前の0でない差分
	mtValCnt = 0;						// 山谷検出数カウンタ
	afLfDiffSav = 0;					// 以前の0でない差分
	mtLfValCnt = 0;						// 山谷検出数カウンタ
	mtValJudg = 10;						// レンズ端判定値

	overPeakCount = 0;					// ピーク検出後に進んだフレーム数
	afHfEvalPeak = 0;					// ピーク時のAF評価値
	afHfDiffPeak = 0;
	afEvalPeakRatio = 95;				// ピークに戻る時のAF評価値比率(%)
	returnPeakCount = 0;				// ピークに戻る時のカウンタ
	afEvalPeakJudge = 0;				// ピークに戻る時のAF評価値

	HighSpeedCount = 0;
	LowSpeedCount = 0;

	afHfEvalReboot = 0;					// 再起動時のHF評価値
	afLfEvalReboot = 0;					// 再起動時のLF評価値
	afChngSpedThresh = 8000;				// 
	afOverPeakThresh = 5000;
	afEvalStop = 0;
	afEvalRebootThreshRatio = 50;			// 比率に修正
	afEvalRebootThresh = 0;
	afRebootCount = 0;

	p_spec = NULL;	

	zoomControl = 0;

}

AXMIspAfBase::~AXMIspAfBase() {
}

void AXMIspAfBase::initAfSpec(ST_SPEC *ptrSpec) {
	p_spec = ptrSpec;
}

void AXMIspAfBase::setAfSpec(void) {
	zoomControl = 0;

	if(p_spec) {
		if(p_spec->changeStatus[1] & SPEC_CHG_AF ) {
			 afStartDir = p_spec->AF.afStartDir;							// 初動方向
			afStartDirChangeThreshHf = 5000;
			afStartDirChangeThreshLf = 5000;
			afStartDirChangeThreshRatio = p_spec->AF.afStartDirChangeThresh;		// 開始時方向切替判定閾値
			afStartDirJudMax = p_spec->AF.afStartDirJudMax;					// 開始時方向切替判定所定回数
			lensAtEndJudg = p_spec->AF.lensAtEndJudg;						// レンズが端にあるか判定用
			afArea = p_spec->AF.focusArea;										// オートフォーカス領域指定0～8
			afEvalPeakRatio = p_spec->AF.afEvalPeakRatio;					// ピークに戻る時のAF評価値比率(%)
			afChngSpedThresh = p_spec->AF.afChngSpedThresh;					// 
			afOverPeakThreshRatio = p_spec->AF.afOverPeakThresh;
			afEvalRebootThreshRatio = p_spec->AF.afEvalRebootThresh;
			afOneShotEnable = p_spec->AF.afOneShotEnable;

			p_spec->changeStatus[1] &= ~SPEC_CHG_AF;
		}
		if(p_spec->changeStatus[1] & SPEC_CHG_ZOOMIN ) {
			zoomControl = ZOOM_CONT_ZOOMIN_FAST;
			p_spec->changeStatus[1] &= ~SPEC_CHG_ZOOMIN;
		}
		if(p_spec->changeStatus[1] & SPEC_CHG_ZOOMOUT ) {
			zoomControl = ZOOM_CONT_ZOOMOUT_FAST;
			p_spec->changeStatus[1] &= ~SPEC_CHG_ZOOMOUT;
		}
		if(p_spec->changeStatus[1] & SPEC_CHG_AFONESHOT ) {
//			if(afOneShotEnable) {
				afOneShotStart = 1;
//			}
			p_spec->changeStatus[1] &= ~SPEC_CHG_AFONESHOT;
		}
	}
}

void AXMIspAfBase::notifyAfEval(long *obj) {

	if(obj) {
		for(int i = 0; i < 9 ; i++ ) {
#ifdef AF_DEBUG_LOG
//			((long *)evalLogTable[i])[st_log_index] =
#endif
				afHfEval[i] = obj[i];
#ifdef AF_DEBUG_LOG
//			((long *)evalLogTable[i+9])[st_log_index] =
#endif
				afLfEval[i] = obj[i+9];
		}
#ifdef AF_DEBUG_LOG
		st_count_log[st_log_index] = obj[18];
		st_shutter_log[st_log_index] = obj[19];			// 20
		st_gain_log[st_log_index] = obj[20];			// 19
		st_vmax_log[st_log_index] = obj[21];			// 19

#endif
	}
}

void AXMIspAfBase::AfExec(void) {
	controlAF.lensMove &= 0x0F;			// ZOOM制御をクリアする
#ifdef AF_DEBUG_LOG
	st_AfStatus_log[st_log_index] = status;
	st_lowEval_log[st_log_index] = afLfEval[afArea];
#endif
	switch(status) {
	case AF_STS_DIRJUDG:
		StartDirectionJudge();			// (1)開始時方向判定
		break;
	case AF_STS_DIRJUDG_RETURN:
		StartDirectionJudgeReturn();	// (7)開始時方向判定
		break;
	case AF_STS_HIGHSPEED:
		HighSpeedDrive();				// (2)高速駆動
		break;
	case AF_STS_LOWSPEED:
		LowSpeedDrive();				// (3)低速駆動（変曲点でブレーキング／省略する場合もあり）
		break;
	case AF_STS_PEAKDETECT:
		PeakDetection();				// (4)頂点検知
		break;
	case AF_STS_RETANDSTOP:
		ReturnAndStop();				// (5)戻って停止
		break;
	case AF_STS_REBOOT:
		Reboot();						// (6)再起動
		break;
	default:
		Debug();
		break;

	}
#ifdef AF_DEBUG_LOG
	st_log_index = (st_log_index+1) & (LOGMAX-1);
#endif
	controlAF.lensMove |= zoomControl;
}

void AXMIspAfBase::StartDirectionJudge(void) {
#ifdef AF_DEBUG_LOG
st_Eval_log[st_log_index] = afHfEval[afArea];
st_path_log[st_log_index] = PATHLOG_StartDirectionJudge_000;
st_deciValue1_log[st_log_index] = afHfEvalReboot;
st_deciValue2_log[st_log_index] = afLfEvalReboot;
#endif
	if(afStartDir == AF_STADIR_TOMACRO) {
		controlAF.lensMove = AF_CONT_TOMACRO_FAST;						// (1)レンズ高速駆動(マクロ方向)
#ifdef AF_DEBUG_LOG
st_path_log[st_log_index] |= PATHLOG_StartDirectionJudge_001;
#endif
	} else {
		controlAF.lensMove = AF_CONT_TOINF_FAST;						// (4)レンズ高速駆動(無限遠方向)
#ifdef AF_DEBUG_LOG
st_path_log[st_log_index] |= PATHLOG_StartDirectionJudge_002;
#endif
	}
	if(afHfEval[afArea] != 0) {
#ifdef AF_DEBUG_LOG
st_path_log[st_log_index] |= PATHLOG_StartDirectionJudge_003;
#endif
		if(afHfEval3 == 0) {
			afHfEval2 = afHfEval3 = afHfEval[afArea];
		}
		afHfEval1 = afHfEval2;
		afHfEval2 = afHfEval3;
		afHfEval3 = afHfEval[afArea];
	}
	if(afLfEval[afArea] != 0){
#ifdef AF_DEBUG_LOG
st_path_log[st_log_index] |= PATHLOG_StartDirectionJudge_004;
#endif
		if(afLfEval3 == 0) {
			afLfEval2 = afLfEval3 = afLfEval[afArea];
		}
		afLfEval1 = afLfEval2;
		afLfEval2 = afLfEval3;
		afLfEval3 = afLfEval[afArea];
	}

	if((afHfEval[afArea] != 0) || (afLfEval[afArea] != 0)) {

		if(afStartDirectJudgCount < afStartDirJudMax) {
//＝＝＝ 指定回数内 ＝＝＝
#ifdef AF_DEBUG_LOG
st_path_log[st_log_index] |= PATHLOG_StartDirectionJudge_005;
#endif
			if(((afHfEval[afArea] != 0) && ((afHfEvalReboot - afHfEval[afArea]) >= afStartDirChangeThreshHf)) || 
			   ((afLfEval[afArea] != 0) && ((afLfEvalReboot - afLfEval[afArea]) >= afStartDirChangeThreshLf))) {
#ifdef AF_DEBUG_LOG
st_path_log[st_log_index] |= PATHLOG_StartDirectionJudge_006;
st_deciValue1_log[st_log_index] = afHfEvalReboot;
st_deciValue2_log[st_log_index] = afLfEvalReboot;
#endif
			   	//方向変換する
				if(controlAF.lensMove == AF_CONT_TOMACRO_FAST) {
					controlAF.lensMove = AF_CONT_TOINF_FAST;				// (4)レンズ高速駆動(無限遠方向)
#ifdef AF_DEBUG_LOG
st_path_log[st_log_index] |= PATHLOG_StartDirectionJudge_007;
#endif
				} else {
					controlAF.lensMove = AF_CONT_TOMACRO_FAST;				// (1)レンズ高速駆動(マクロ方向)
#ifdef AF_DEBUG_LOG
st_path_log[st_log_index] |= PATHLOG_StartDirectionJudge_008;
#endif
				}
#ifdef AF_DEBUG_LOG
st_Eval_log[st_log_index] = afHfEval[afArea];
#endif
				// 遷移条件：指定回数内に評価値が指定の閾値以下になった
				status = AF_STS_HIGHSPEED;
				afHfEvalChgStatus = 0;
				HighSpeedCount = detectEndFlag = evalZeroCount = mtValCnt = detectChgDirFlag = 0;
				afHfEval1 = afHfEval2 = afHfEval3 = afHfEvalPeak = afHfDiffPeak = 0;
			   	afHfDiffValley = 0x7FFFFFFFL;
				afEvalPeakJudge = 0x7FFFFFFFL;	// LONG 最大値
			}
			long	afHfDiff2 = afHfEval3 - afHfEval2;

			if((afHfEval3 > 5000) && (((afHfDiff2 * 1000) / afHfEval3) > afChngSpedThresh)) {
#ifdef AF_DEBUG_LOG
st_path_log[st_log_index] |= PATHLOG_StartDirectionJudge_009;
#endif
					detectChgSpedFlag = 1;
			}
			afStartDirectJudgCount++;
		} else {
//
//指定回数以上
//
			if(detectChgSpedFlag == 0) {
#ifdef AF_DEBUG_LOG
st_path_log[st_log_index] |= PATHLOG_StartDirectionJudge_00A;
#endif
				status = AF_STS_HIGHSPEED;
				afHfEvalChgStatus = 0;
				HighSpeedCount = detectEndFlag = evalZeroCount = mtValCnt = detectChgDirFlag = 0;
				afHfEval1 = afHfEval2 = afHfEval3 = afHfEvalPeak = afHfDiffPeak = 0;
			   	afHfDiffValley = 0x7FFFFFFFL;
				afEvalPeakJudge = 0x7FFFFFFFL;	// LONG 最大値
			} else {
				status = AF_STS_DIRJUDG_RETURN;
			   	//方向変換する
				if(controlAF.lensMove == AF_CONT_TOMACRO_FAST) {
					controlAF.lensMove = AF_CONT_TOINF_FAST;				// (4)レンズ高速駆動(無限遠方向)
#ifdef AF_DEBUG_LOG
st_path_log[st_log_index] |= PATHLOG_StartDirectionJudge_00B;
#endif
				} else {
					controlAF.lensMove = AF_CONT_TOMACRO_FAST;				// (1)レンズ高速駆動(マクロ方向)
#ifdef AF_DEBUG_LOG
st_path_log[st_log_index] |= PATHLOG_StartDirectionJudge_00C;
#endif
				}
			}
		}
	} else {
#ifdef AF_DEBUG_LOG
st_path_log[st_log_index] |= PATHLOG_StartDirectionJudge_00D;
#endif
		if(evalZeroCount++ > 300) {
#ifdef AF_DEBUG_LOG
st_path_log[st_log_index] |= PATHLOG_StartDirectionJudge_00E;
#endif
			status = AF_STS_REBOOT;			// (6)再起動
			afEvalStop = 0;
			controlAF.lensMove = AF_CONT_STOP;
		}
	}

}

void AXMIspAfBase::StartDirectionJudgeReturn(void) {
#ifdef AF_DEBUG_LOG
st_path_log[st_log_index] = PATHLOG_StartDirectionJudgeReturn_000;
st_Eval_log[st_log_index] = afHfEval[afArea];
st_deciValue1_log[st_log_index] = afHfEval[afArea];
st_deciValue2_log[st_log_index] = afLfEval[afArea];
#endif
	if(afStartDirectJudgCount-- > 0) {
#ifdef AF_DEBUG_LOG
st_path_log[st_log_index] |= PATHLOG_StartDirectionJudgeReturn_001;
#endif
	} else {
#ifdef AF_DEBUG_LOG
st_path_log[st_log_index] |= PATHLOG_StartDirectionJudgeReturn_002;
#endif
	   	//方向変換する
		if(controlAF.lensMove == AF_CONT_TOMACRO_FAST) {
			controlAF.lensMove = AF_CONT_TOINF_FAST;				// (4)レンズ高速駆動(無限遠方向)
#ifdef AF_DEBUG_LOG
st_path_log[st_log_index] |= PATHLOG_StartDirectionJudgeReturn_003;
#endif
		} else {
			controlAF.lensMove = AF_CONT_TOMACRO_FAST;				// (1)レンズ高速駆動(マクロ方向)
#ifdef AF_DEBUG_LOG
st_path_log[st_log_index] |= PATHLOG_StartDirectionJudgeReturn_004;
#endif
		}
		// 遷移条件：指定回数内に評価値が指定の閾値以下になった
		status = AF_STS_HIGHSPEED;
		afHfEvalChgStatus = 0;
		HighSpeedCount = detectEndFlag = evalZeroCount = mtValCnt = detectChgDirFlag = 0;
		afHfEval1 = afHfEval2 = afHfEval3 = afHfEvalPeak = afHfDiffPeak = 0;
	   	afHfDiffValley = 0x7FFFFFFFL;
		afEvalPeakJudge = 0x7FFFFFFFL;	// LONG 最大値
	}
}

void AXMIspAfBase::HighSpeedDrive(void) {
	if(afHfEvalChgStatus == 0) {
		afHfEvalChgStatus = afHfEval[afArea];
	}
#ifdef AF_DEBUG_LOG
st_Eval_log[st_log_index] = afHfEval[afArea];
st_path_log[st_log_index] = PATHLOG_HighSpeedDrive_000;
st_deciValue1_log[st_log_index] = afHfEvalChgStatus;
st_deciValue2_log[st_log_index] = afHfEval3;
#endif
	if(afHfEval[afArea] != 0) {
#ifdef AF_DEBUG_LOG
st_path_log[st_log_index] |= PATHLOG_HighSpeedDrive_001;
#endif
		if(afHfEval3 == 0) {
#ifdef AF_DEBUG_LOG
st_path_log[st_log_index] |= PATHLOG_HighSpeedDrive_002;
#endif
			afHfEval2 = afHfEval3 = afHfEval[afArea];
		}
		evalZeroCount = 0;
		afHfEval1 = afHfEval2;
		afHfEval2 = afHfEval3;
		afHfEval3 = afHfEval[afArea];
		if((afHfEval1 != 0) && (afHfEval2 != 0) && (afHfEval3 != 0)) {
			long	afHfDiff1 = afHfEval2 - afHfEval1;
			long	afHfDiff2 = afHfEval3 - afHfEval2;
			long	afHfDiffDiff = afHfDiff2 - afHfDiff1;
#ifdef AF_DEBUG_LOG
st_path_log[st_log_index] |= PATHLOG_HighSpeedDrive_003;
#endif
			if(afHfEvalPeak <= afHfEval3) {
#ifdef AF_DEBUG_LOG
st_path_log[st_log_index] |= PATHLOG_HighSpeedDrive_004;
st_deciValue1_log[st_log_index] = afHfEvalPeak;
#endif
				afHfEvalPeak = afHfEval3;
				overPeakCount = 0;
			}

#ifdef AF_DEBUG_LOG
st_deciValue1_log[st_log_index] = afHfEval3;
st_deciValue2_log[st_log_index] = ((afHfDiff2 * 1000) / afHfEval3);
#endif
			if((detectChgDirFlag == 0) && ((((afHfEvalChgStatus - afHfEval3) * 100) / afHfEvalChgStatus) > 30)) {
				detectChgDirFlag = 1;
				if(controlAF.lensMove == AF_CONT_TOINF_FAST) {
					controlAF.lensMove = AF_CONT_TOMACRO_FAST;
#ifdef AF_DEBUG_LOG
st_path_log[st_log_index] |= PATHLOG_HighSpeedDrive_005;
#endif
				} else {
					controlAF.lensMove = AF_CONT_TOINF_FAST;
#ifdef AF_DEBUG_LOG
st_path_log[st_log_index] |= PATHLOG_HighSpeedDrive_006;
#endif
				}
			}
//			if((afHfDiff1 > 0) && (((afHfDiffPeak >= afChngSpedThresh) && (afHfDiffPeak > afHfDiffDiff)) || (afHfEval3 >= afEvalPeakJudge)) ) 
			else if((afHfEval3 > 5000) && ((((afHfDiff2 * 1000) / afHfEval3) > afChngSpedThresh) || (afHfEval3 >= afEvalPeakJudge)) ) 
			{
#ifdef AF_DEBUG_LOG
st_path_log[st_log_index] |= PATHLOG_HighSpeedDrive_007;
st_deciValue1_log[st_log_index] = afHfDiff1;
st_deciValue2_log[st_log_index] = afHfDiff2;
#endif
				if(controlAF.lensMove == AF_CONT_TOINF_FAST) {
					controlAF.lensMove = AF_CONT_TOINF_SLOW;
#ifdef AF_DEBUG_LOG
st_path_log[st_log_index] |= PATHLOG_HighSpeedDrive_008;
#endif
				} else {
					controlAF.lensMove = AF_CONT_TOMACRO_SLOW;
#ifdef AF_DEBUG_LOG
st_path_log[st_log_index] |= PATHLOG_HighSpeedDrive_009;
#endif
				}
				status = AF_STS_LOWSPEED;	// (3)低速駆動に遷移
				LowSpeedCount = 0;
				if(afHfDiff2 < 0) {	// ピーク越え？
#ifdef AF_DEBUG_LOG
st_path_log[st_log_index] |= PATHLOG_HighSpeedDrive_00A;
#endif
					afHfEvalPeak = afHfEval2;
					overPeakCount = 6;	// 高速２回分
				} else {
#ifdef AF_DEBUG_LOG
st_path_log[st_log_index] |= PATHLOG_HighSpeedDrive_00B;
#endif
					afHfEvalPeak = afHfEval3;
					overPeakCount = 3;	// 高速1回分
				}
			} else if((HighSpeedCount > lensAtEndJudg) /*|| (mtValCnt > mtValJudg)*/ /*|| ((afHfEvalChgStatus - afHfEval3) >= HighSpeedChangeDirThresh)*/) {
#ifdef AF_DEBUG_LOG
st_path_log[st_log_index] |= PATHLOG_HighSpeedDrive_00C;
st_deciValue1_log[st_log_index] = HighSpeedCount;
st_deciValue2_log[st_log_index] = (afHfEvalChgStatus - afHfEval3);
#endif
				if(detectEndFlag) {
#ifdef AF_DEBUG_LOG
st_path_log[st_log_index] |= PATHLOG_HighSpeedDrive_00D;
#endif
					afEvalPeakJudge = ((afHfEvalPeak * afEvalPeakRatio) + 50) / 100;
				}
				detectEndFlag = 1;
				afHfEvalChgStatus = afHfEval3;
				HighSpeedCount = 0;
				afHfEval1 = afHfEval2 = afHfEval3 = afHfEvalPeak = 0;

				if(controlAF.lensMove == AF_CONT_TOMACRO_FAST) {
					controlAF.lensMove = AF_CONT_TOINF_FAST;				// (4)レンズ高速駆動(無限遠方向)
#ifdef AF_DEBUG_LOG
st_path_log[st_log_index] |= PATHLOG_HighSpeedDrive_00E;
#endif
				} else {
					controlAF.lensMove = AF_CONT_TOMACRO_FAST;				// (1)レンズ高速駆動(マクロ方向)
#ifdef AF_DEBUG_LOG
st_path_log[st_log_index] |= PATHLOG_HighSpeedDrive_00F;
#endif
				}
			} else {
#ifdef AF_DEBUG_LOG
st_path_log[st_log_index] |= PATHLOG_HighSpeedDrive_010;
#endif
				HighSpeedCount ++;
				if(afHfDiffPeak > afHfDiffDiff) {	// 微分ピーク検知
#ifdef AF_DEBUG_LOG
st_path_log[st_log_index] |= PATHLOG_HighSpeedDrive_011;
#endif
					afHfDiffValley = 0x7FFFFFFFL;	// 微分最小値リセット
				}
				if(afHfDiffValley < afHfDiffDiff) {	// 微分谷検知
#ifdef AF_DEBUG_LOG
st_path_log[st_log_index] |= PATHLOG_HighSpeedDrive_012;
#endif
					afHfDiffPeak = 0;				// 微分最大値リセット
				}
				if(afHfDiffPeak < afHfDiffDiff) {	// 微分最大値更新
#ifdef AF_DEBUG_LOG
st_path_log[st_log_index] |= PATHLOG_HighSpeedDrive_013;
#endif
					afHfDiffPeak = afHfDiffDiff;
				}
				if(afHfDiffValley > afHfDiffDiff) {	// 微分最小値更新
#ifdef AF_DEBUG_LOG
st_path_log[st_log_index] |= PATHLOG_HighSpeedDrive_014;
#endif
					afHfDiffValley = afHfDiffDiff;
				}

			}
		}
	} else {
#ifdef AF_DEBUG_LOG
st_path_log[st_log_index] |= PATHLOG_HighSpeedDrive_015;
#endif
		if(evalZeroCount++ > 300) {
#ifdef AF_DEBUG_LOG
st_path_log[st_log_index] |= PATHLOG_HighSpeedDrive_016;
#endif
			status = AF_STS_REBOOT;			// (6)再起動
			afEvalStop = 0;
			controlAF.lensMove = AF_CONT_STOP;
		}
	}
}

void AXMIspAfBase::LowSpeedDrive(void) {
#ifdef AF_DEBUG_LOG
st_Eval_log[st_log_index] = afHfEval[afArea];
st_path_log[st_log_index] = PATHLOG_LowSpeedDrive_000;
st_deciValue1_log[st_log_index] = 0;
st_deciValue2_log[st_log_index] = 0;
#endif
	if(afHfEval[afArea] != 0) {
#ifdef AF_DEBUG_LOG
st_path_log[st_log_index] |= PATHLOG_LowSpeedDrive_001;
#endif
		afHfEval1 = afHfEval2;
		afHfEval2 = afHfEval[afArea];
		if(afHfEval1 != 0 && afHfEval2 != 0) {
#ifdef AF_DEBUG_LOG
st_path_log[st_log_index] |= PATHLOG_LowSpeedDrive_002;
#endif
			long	afHfDiff1 = afHfEval2 - afHfEval1;
			
			if(afHfEvalPeak <= afHfEval2) {
#ifdef AF_DEBUG_LOG
st_path_log[st_log_index] |= PATHLOG_LowSpeedDrive_003;
st_deciValue1_log[st_log_index] = afHfEvalPeak;
st_deciValue2_log[st_log_index] = afOverPeakThresh;
#endif
				afHfEvalPeak = afHfEval2;
				afOverPeakThresh = ((afHfEvalPeak * afOverPeakThreshRatio) + 50) / 100;
				overPeakCount = 0;
			} else {
#ifdef AF_DEBUG_LOG
st_path_log[st_log_index] |= PATHLOG_LowSpeedDrive_004;
st_deciValue1_log[st_log_index] = afHfEvalPeak;
st_deciValue2_log[st_log_index] = afOverPeakThresh;
#endif
				overPeakCount ++;
			}
			if((afHfDiff1 < 0) && ((afHfEvalPeak - afHfEval2) >= afOverPeakThresh)) {
				status = AF_STS_PEAKDETECT;		// (4)頂点検知
#ifdef AF_DEBUG_LOG
st_path_log[st_log_index] |= PATHLOG_LowSpeedDrive_005;
st_deciValue1_log[st_log_index] = afHfEvalPeak;
st_deciValue2_log[st_log_index] = afOverPeakThresh;
#endif
			}
		}
		if(LowSpeedCount++ > 300) {
#ifdef AF_DEBUG_LOG
st_path_log[st_log_index] |= PATHLOG_LowSpeedDrive_006;
#endif
			status = AF_STS_DIRJUDG;			// (1)開始時方向判定
			afStartDirectJudgCount = evalZeroCount = 0;
			afHfDiffSav = mtValCnt = afLfDiffSav = mtLfValCnt = 0;
			detectChgSpedFlag = 0;
			afHfEvalReboot = afHfEval[afArea];
			afStartDirChangeThreshHf = ((afHfEvalReboot * afStartDirChangeThreshRatio) + 50) / 100;
			afLfEvalReboot = afLfEval[afArea];
			afStartDirChangeThreshLf = ((afLfEvalReboot * afStartDirChangeThreshRatio) + 50) / 100;
			afStartDir = (afStartDir == AF_STADIR_TOMACRO)  ? AF_STADIR_TOINF : AF_STADIR_TOMACRO;
		}
	}
}

void AXMIspAfBase::PeakDetection(void) {
	 //m_log.write(AXFLOG_DEBUG, "PeakDetection() ");
#ifdef AF_DEBUG_LOG
st_Eval_log[st_log_index] = afHfEval[afArea];
st_path_log[st_log_index] = PATHLOG_PeakDetection_000;
st_deciValue1_log[st_log_index] = 0;
st_deciValue2_log[st_log_index] = 0;
#endif
	if(afHfEval[afArea] != 0) {
#ifdef AF_DEBUG_LOG
st_path_log[st_log_index] |= PATHLOG_PeakDetection_001;
#endif
		long afHfDiff1 = afHfEvalPeak - afHfEval[afArea];
		if(afHfDiff1 < 0) {
#ifdef AF_DEBUG_LOG
st_path_log[st_log_index] |= PATHLOG_PeakDetection_002;
#endif
			afHfDiff1 = -afHfDiff1;
		}
		overPeakCount ++;
		if(afHfDiff1 >= afOverPeakThresh) {
			status = AF_STS_RETANDSTOP;		// (5)戻って停止
			returnPeakCount = 0;
			afEvalPeakJudge = ((afHfEvalPeak * afEvalPeakRatio) + 50) / 100;
#ifdef AF_DEBUG_LOG
st_path_log[st_log_index] |= PATHLOG_PeakDetection_003;
st_deciValue1_log[st_log_index] = overPeakCount;
st_deciValue2_log[st_log_index] = afEvalPeakJudge;
#endif
			if(controlAF.lensMove == AF_CONT_TOINF_SLOW) {
				controlAF.lensMove = AF_CONT_TOMACRO_SLOW;
#ifdef AF_DEBUG_LOG
st_path_log[st_log_index] |= PATHLOG_PeakDetection_004;
#endif
			} else {
				controlAF.lensMove = AF_CONT_TOINF_SLOW;
#ifdef AF_DEBUG_LOG
st_path_log[st_log_index] |= PATHLOG_PeakDetection_005;
#endif
			}
		}
		if(overPeakCount > 200) {
#ifdef AF_DEBUG_LOG
st_path_log[st_log_index] |= PATHLOG_PeakDetection_006;
#endif
			status = AF_STS_DIRJUDG;			// (1)開始時方向判定
			afStartDirectJudgCount = evalZeroCount = 0;
			afHfDiffSav = mtValCnt = afLfDiffSav = mtLfValCnt = 0;
			detectChgSpedFlag = 0;
			afHfEvalReboot = afHfEval[afArea];
			afStartDirChangeThreshHf = ((afHfEvalReboot * afStartDirChangeThreshRatio) + 50) / 100;
			afLfEvalReboot = afLfEval[afArea];
			afStartDirChangeThreshLf = ((afLfEvalReboot * afStartDirChangeThreshRatio) + 50) / 100;
			afStartDir = (afStartDir == AF_STADIR_TOMACRO)  ? AF_STADIR_TOINF : AF_STADIR_TOMACRO;
		}
	}
}

void AXMIspAfBase::ReturnAndStop(void) {
#ifdef AF_DEBUG_LOG
st_Eval_log[st_log_index] = afHfEval[afArea];
st_path_log[st_log_index] = PATHLOG_ReturnAndStop_000;
st_deciValue1_log[st_log_index] = returnPeakCount;
st_deciValue2_log[st_log_index] = 0;
#endif
	if((returnPeakCount++ >= overPeakCount) /* || (afHfEval[afArea] >= afEvalPeakJudge)*/) {
#ifdef AF_DEBUG_LOG
st_path_log[st_log_index] |= PATHLOG_ReturnAndStop_001;
#endif
		status = AF_STS_REBOOT;			// (6)再起動
		afEvalStop = 0;
		controlAF.lensMove = AF_CONT_STOP;
#ifdef AF_DEBUG_LOG
st_deciValue1_log[st_log_index] = afEvalStop;
#endif
	}
}

void AXMIspAfBase::Reboot(void) {
#ifdef AF_DEBUG_LOG
st_Eval_log[st_log_index] = afHfEval[afArea];
st_path_log[st_log_index] = PATHLOG_Reboot_000;
st_deciValue1_log[st_log_index] = 0;
st_deciValue2_log[st_log_index] = 0;
#endif
	if(afHfEval[afArea] != 0) {
		if(afEvalStop == 0) {
			afEvalStop = afHfEval[afArea];
			afEvalRebootThresh = ((afEvalStop * afEvalRebootThreshRatio) + 50) / 100;
				afRebootCount = 0;
		} else  {
			if(afOneShotStart) {
				afOneShotStart = 0;
				status = AF_STS_DIRJUDG;			// (1)開始時方向判定
				afStartDirectJudgCount = evalZeroCount = 0;
				afHfDiffSav = mtValCnt = afLfDiffSav = mtLfValCnt = 0;
				detectChgSpedFlag = 0;
				afHfEvalReboot = afHfEval[afArea];
				afStartDirChangeThreshHf = ((afHfEvalReboot * afStartDirChangeThreshRatio) + 50) / 100;
				afLfEvalReboot = afLfEval[afArea];
				afStartDirChangeThreshLf = ((afLfEvalReboot * afStartDirChangeThreshRatio) + 50) / 100;
				afStartDir = (afStartDir == AF_STADIR_TOMACRO)  ? AF_STADIR_TOINF : AF_STADIR_TOMACRO;

#ifdef AF_DEBUG_LOG
st_path_log[st_log_index] |= PATHLOG_Reboot_001;
st_deciValue1_log[st_log_index] = afHfEvalReboot;
st_deciValue2_log[st_log_index] = afLfEvalReboot;
#endif
			} else if(afOneShotEnable == 0) {
				long afHfDiff1 = (afEvalStop > afHfEval[afArea]) ? (afEvalStop - afHfEval[afArea]) : (afHfEval[afArea] - afEvalStop);
#ifdef AF_DEBUG_LOG
st_deciValue1_log[st_log_index] = afHfDiff1;
#endif
				if(afHfDiff1 >= afEvalRebootThresh) {
#ifdef AF_DEBUG_LOG
st_path_log[st_log_index] |= PATHLOG_Reboot_002;
#endif
					afRebootCount ++;
				} else {
#ifdef AF_DEBUG_LOG
st_path_log[st_log_index] |= PATHLOG_Reboot_003;
#endif
					afRebootCount = 0;
				}
				if(afRebootCount > 40) {
					status = AF_STS_DIRJUDG;			// (1)開始時方向判定
					afStartDirectJudgCount = evalZeroCount = 0;
					afHfDiffSav = mtValCnt = afLfDiffSav = mtLfValCnt = 0;
					detectChgSpedFlag = 0;
					afHfEvalReboot = afHfEval[afArea];
					afStartDirChangeThreshHf = ((afHfEvalReboot * afStartDirChangeThreshRatio) + 50) / 100;
					afLfEvalReboot = afLfEval[afArea];
					afStartDirChangeThreshLf = ((afLfEvalReboot * afStartDirChangeThreshRatio) + 50) / 100;
					afStartDir = (afStartDir == AF_STADIR_TOMACRO)  ? AF_STADIR_TOINF : AF_STADIR_TOMACRO;
				}
#ifdef AF_DEBUG_LOG
st_path_log[st_log_index] |= PATHLOG_Reboot_004;
st_deciValue1_log[st_log_index] = afHfEvalReboot;
st_deciValue2_log[st_log_index] = afLfEvalReboot;
#endif
			}
		}  // end of else
	} // end of if(afHfEval[afArea] != 0)
}


void AXMIspAfBase::Debug(void) {
	static int cnt = 0;
#ifdef AF_DEBUG_LOG
st_Eval_log[st_log_index] = afHfEval[afArea];
st_path_log[st_log_index] = PATHLOG_Debug_000;
st_deciValue1_log[st_log_index] = 0;
st_deciValue2_log[st_log_index] = cnt;
#endif

		switch(status) {
		case AF_STS_DEBUG_TOINF_FAST:
		default:
			controlAF.lensMove = AF_CONT_TOINF_FAST;						// (4)レンズ高速駆動(無限遠方向)
#ifdef AF_DEBUG_LOG
st_path_log[st_log_index] |= PATHLOG_Debug_001;
st_deciValue1_log[st_log_index] = controlAF.lensMove;
#endif
			debugSupport();
			if(++cnt >= 300){	// 2秒で移動
				status = AF_STS_DEBUG_TOMACRO_FAST;
				cnt = 0;
				if(afHfDiffPeak < 4000) {
					debugDetect();
				}
				afHfEval1 = afHfEval2 = afHfEval3 = 0;
				afHfDiffValley = 0x7FFFFFFFL;	// 微分最小値リセット
				afHfDiffPeak = 0;				// 微分最大値リセット
			}
			break;
		case AF_STS_DEBUG_TOMACRO_FAST:
			controlAF.lensMove = AF_CONT_TOMACRO_FAST;						// (1)レンズ高速駆動(マクロ方向)
#ifdef AF_DEBUG_LOG
st_path_log[st_log_index] |= PATHLOG_Debug_002;
st_deciValue1_log[st_log_index] = controlAF.lensMove;
#endif
			debugSupport();
			if(++cnt >= 300){	// 2秒で移動
				status = AF_STS_DEBUG_TOINF_FAST;
				cnt = 0;
				if(afHfDiffPeak < 4000) {
					debugDetect();
				}
				afHfEval1 = afHfEval2 = afHfEval3 = 0;
				afHfDiffValley = 0x7FFFFFFFL;	// 微分最小値リセット
				afHfDiffPeak = 0;				// 微分最大値リセット
			}
			break;
		case AF_STS_DEBUG_TOINF_SLOW:
			controlAF.lensMove = AF_CONT_TOINF_SLOW;						// (2)レンズ低速駆動(マクロ方向)
#ifdef AF_DEBUG_LOG
st_path_log[st_log_index] |= PATHLOG_Debug_003;
st_deciValue1_log[st_log_index] = controlAF.lensMove;
#endif
			if(++cnt >= 900 ){	// 6秒で移動 256 => 1
				status = AF_STS_DEBUG_MOVE_AND_STOP;
				cnt = 0;
			}
			break;
		case AF_STS_DEBUG_TOMACRO_SLOW:
			controlAF.lensMove = AF_CONT_TOMACRO_SLOW;						// (3)レンズ低速駆動(無限遠方向)
#ifdef AF_DEBUG_LOG
st_path_log[st_log_index] |= PATHLOG_Debug_004;
st_deciValue1_log[st_log_index] = controlAF.lensMove;
#endif
			if(++cnt >= 900){	// 6秒で移動 256 => 1
				status = AF_STS_DEBUG_MOVE_AND_STOP;
				cnt = 0;
			}
			break;
		case AF_STS_DEBUG_MOVE_AND_STOP:
				controlAF.lensMove = AF_CONT_STOP;
#ifdef AF_DEBUG_LOG
st_path_log[st_log_index] |= PATHLOG_Debug_005;
st_deciValue1_log[st_log_index] = controlAF.lensMove;
#endif
//			if(++cnt >= 256){	// 6秒で移動
//				status = AF_STS_DEBUG_TOINF_FAST;
//				cnt = 0;
//			}
			break;
		case AF_STS_DEBUG_TOINF_FAST_FOREVER:
			controlAF.lensMove = AF_CONT_TOINF_FAST;						// (4)レンズ高速駆動(無限遠方向)
			break;
		case AF_STS_DEBUG_TOMACRO_FAST_FOREVER:
			controlAF.lensMove = AF_CONT_TOMACRO_FAST;						// (1)レンズ高速駆動(マクロ方向)
			break;
		case AF_STS_DEBUG_TOINF_SLOW_FOREVER:
			controlAF.lensMove = AF_CONT_TOINF_SLOW;					
			break;
		case AF_STS_DEBUG_TOMACRO_SLOW_FOREVER:
			controlAF.lensMove = AF_CONT_TOMACRO_SLOW;
			break;
		case AF_STS_DEBUG_STOP_FOREVER:
			controlAF.lensMove = AF_CONT_STOP;
			break;

		case AF_STS_DEBUG_FAST_START:
			afHfEval1 = afHfEval2 = afHfEval3 = 0;
			afHfDiffValley = 0x7FFFFFFFL;	// 微分最小値リセット
			afHfDiffPeak = 0;				// 微分最大値リセット
			status = AF_STS_DEBUG_TOINF_FAST;
			break;
		case AF_STS_DEBUG_TOINF_SLOW_START:
			status = AF_STS_DEBUG_TOINF_SLOW;
#ifdef AF_DEBUG_LOG
	st_log_index = 0;
#endif
			break;
		case AF_STS_DEBUG_TOMACRO_SLOW_START:
			status = AF_STS_DEBUG_TOMACRO_SLOW;
#ifdef AF_DEBUG_LOG
	st_log_index = 0;
#endif
			break;

		}
}

void AXMIspAfBase::debugSupport(void) {
	if(afHfEval[afArea] != 0) {
		if(afHfEval3 == 0) {
			afHfEval2 = afHfEval3 = afHfEval[afArea];
		}

		afHfEval1 = afHfEval2;
		afHfEval2 = afHfEval3;
		afHfEval3 = afHfEval[afArea];
		if((afHfEval1 != 0) && (afHfEval2 != 0) && (afHfEval3 != 0)) {
			long	afHfDiff1 = afHfEval2 - afHfEval1;
			long	afHfDiff2 = afHfEval3 - afHfEval2;
			long	afHfDiffDiff = afHfDiff2 - afHfDiff1;
			if(afHfEvalPeak <= afHfEval3) {
				afHfEvalPeak = afHfEval3;
			}

			if(afHfDiffPeak < afHfDiffDiff) {	// 微分最大値更新
				afHfDiffPeak = afHfDiffDiff;
			}
			if(afHfDiffValley > afHfDiffDiff) {	// 微分最小値更新
				afHfDiffValley = afHfDiffDiff;
			}
		}
	}
}

void  AXMIspAfBase::debugDetect(void) {
	
}

