#Region ;**** Directives created by AutoIt3Wrapper_GUI ****
#AutoIt3Wrapper_UseX64=y
#EndRegion ;**** Directives created by AutoIt3Wrapper_GUI ****

Opt("MustDeclareVars", 1)

#include "emgucv-autoit-bindings\cve_extra.au3"

;start dll opencv
_OpenCV_DLLOpen("libemgucv-windesktop-4.5.3.4721\libs\x64\cvextern.dll")

Local $algorithm = "KAZE"
Local $imgname = "dog.jpg"          ; query image (small object)
Local $imgname2 = "dog-scene.jpg"   ; train image (large scene)

;; Create ORB object and BF object(using HAMMING)
Local $tFeature2DPtr = DllStructCreate("ptr value")
Local $tSharedPtr = DllStructCreate("ptr")
Local $destructor

If StringCompare($algorithm, "KAZE") == 0 Then
	_cveKAZEDetectorCreate(False, False, 0.001, 4, 4, $CV_KAZE_DIFF_PM_G2, $tFeature2DPtr, $tSharedPtr)
	$destructor = "_cveKAZEDetectorRelease"
ElseIf StringCompare($algorithm, "ORB") == 0 Then
	_cveOrbCreate(500, 1.2, 8, 31, 0, 2, $CV_ORB_HARRIS_SCORE, 31, 20, $tFeature2DPtr, $tSharedPtr)
	$destructor = "_cveOrbRelease"
Else
	_cveBriskCreate(30, 3, 1, $tFeature2DPtr, $tSharedPtr)
	$destructor = "_cveBriskRelease"
EndIf

Local $detector = $tFeature2DPtr.value

Local $img1 = _cveImreadAndCheck($imgname, $CV_IMREAD_COLOR)
Local $img2 = _cveImreadAndCheck($imgname2, $CV_IMREAD_COLOR)

Local $grayeq = _cveMatCreate()

Local $gray1 = _cveMatCreate()
_cveCvtColorMat($img1, $gray1, $CV_COLOR_BGR2GRAY)

Local $gray2 = _cveMatCreate()
_cveCvtColorMat($img2, $gray2, $CV_COLOR_BGR2GRAY)

;; Find the keypoints and descriptors with ORB
Local $kpts1 = _VectorOfKeyPointCreate()
Local $kpts2 = _VectorOfKeyPointCreate()
Local $descs1 = _cveMatCreate()
Local $descs2 = _cveMatCreate()

_CvFeature2DDetectAndComputeMat($detector, $gray1, _cveNoArrayMat(), $kpts1, $descs1, False)
ConsoleWrite("object keypoints: " & _VectorOfKeyPointGetSize($kpts1) & @CRLF)
_CvFeature2DDetectAndComputeMat($detector, $gray2, _cveNoArrayMat(), $kpts2, $descs2, False)
ConsoleWrite("scene keypoints: " & _VectorOfKeyPointGetSize($kpts2) & @CRLF)

;; match descriptors and sort them in the order of their distance
Local $tMatcherPtr = DllStructCreate("ptr value")
Local $bf_matcher = _cveBFMatcherCreate($CV_NORM_L1, False, $tMatcherPtr) ;
Local $matcher = $tMatcherPtr.value
Local $matches = _VectorOfDMatchCreate()

_cveDescriptorMatcherMatch1Mat($matcher, $descs1, $descs2, $matches, _cveNoArrayMat())
ConsoleWrite("matches: " & _VectorOfDMatchGetSize($matches) & @CRLF)

Local $dmatches[_VectorOfDMatchGetSize($matches)]
Local $tDMatchPtr = DllStructCreate("ptr value")
For $i = 0 To _VectorOfDMatchGetSize($matches) - 1
	_VectorOfDMatchGetItemPtr($matches, $i, $tDMatchPtr)
	$dmatches[$i] = DllStructCreate($tagCvDMatch, $tDMatchPtr.value)
Next
__ArrayQuickSort($dmatches, "_CompareDistance", 0, UBound($dmatches) - 1)

;; extract the matched keypoints
If UBound($dmatches) >= 4 Then
	;;-- Localize the object
	Local $src_pts = _VectorOfPointFCreate()
	Local $dst_pts = _VectorOfPointFCreate()

	Local $tKpt1Ptr = DllStructCreate("ptr value")
	Local $tKpt2Ptr = DllStructCreate("ptr value")

	For $i = 0 To UBound($dmatches) - 1
		Local $tDMatch = $dmatches[$i]

		_VectorOfKeyPointGetItemPtr($kpts1, $tDMatch.queryIdx, $tKpt1Ptr)
		Local $tKpt1 = DllStructCreate($tagCvKeyPoint, $tKpt1Ptr.value)
		_VectorOfPointFPush($src_pts, $tKpt1)

		_VectorOfKeyPointGetItemPtr($kpts2, $tDMatch.trainIdx, $tKpt2Ptr)
		Local $tKpt2 = DllStructCreate($tagCvKeyPoint, $tKpt2Ptr.value)
		_VectorOfPointFPush($dst_pts, $tKpt2)
	Next

	;; find homography matrix and do perspective transform
	Local $M = _cveMatCreate()
	Local $i_arr_M = _cveInputArrayFromMat($M)
	Local $o_arr_M = _cveOutputArrayFromMat($M)
	Local $i_arr_src = _cveInputArrayFromVectorOfPointF($src_pts)
	Local $i_arr_dst = _cveInputArrayFromVectorOfPointF($dst_pts)
	Local $mask = _cveMatCreate()
	Local $o_arr_mask = _cveOutputArrayFromMat($mask)

	_cveFindHomography($i_arr_src, $i_arr_dst, $o_arr_M, $CV_RANSAC, 5.0, $o_arr_mask)

	_cveOutputArrayRelease($o_arr_mask)
	_cveMatRelease($mask)

	If Not _cveMatIsEmpty($M) Then
		Local $img1_size = _cvSize()
		_cveMatGetSize($img1, $img1_size)
		Local $w = $img1_size.width
		Local $h = $img1_size.height

		Local $pts = _VectorOfPointFCreate()
		Local $i_arr_pts = _cveInputArrayFromVectorOfPointF($pts)

		_VectorOfPointFPush($pts, _cvPoint2f(0, 0))
		_VectorOfPointFPush($pts, _cvPoint2f($w, 0))
		_VectorOfPointFPush($pts, _cvPoint2f($w, $h))
		_VectorOfPointFPush($pts, _cvPoint2f(0, $h))

		Local $dst = _VectorOfPointFCreateSize(4)
		Local $o_arr_dst = _cveOutputArrayFromVectorOfPointF($dst)

		_cvePerspectiveTransform($i_arr_pts, $o_arr_dst, $i_arr_M)

		;; draw found regions
		Local $tPointFPtr = DllStructCreate("ptr value")

		_VectorOfPointFGetItemPtr($dst, 0, $tPointFPtr)
		Local $dst_0 = DllStructCreate($tagCvPoint2D32f, $tPointFPtr.value)

		_VectorOfPointFGetItemPtr($dst, 1, $tPointFPtr)
		Local $dst_1 = DllStructCreate($tagCvPoint2D32f, $tPointFPtr.value)

		_VectorOfPointFGetItemPtr($dst, 2, $tPointFPtr)
		Local $dst_2 = DllStructCreate($tagCvPoint2D32f, $tPointFPtr.value)

		_VectorOfPointFGetItemPtr($dst, 3, $tPointFPtr)
		Local $dst_3 = DllStructCreate($tagCvPoint2D32f, $tPointFPtr.value)

		Local $dst_int32 = _VectorOfPointCreate()
		_VectorOfPointFPush($dst_int32, _cvPoint($dst_0.x, $dst_0.y))
		_VectorOfPointFPush($dst_int32, _cvPoint($dst_1.x, $dst_1.y))
		_VectorOfPointFPush($dst_int32, _cvPoint($dst_2.x, $dst_2.y))
		_VectorOfPointFPush($dst_int32, _cvPoint($dst_3.x, $dst_3.y))

		Local $i_arr_dst_32 = _cveInputArrayFromVectorOfPoint($dst_int32)
		Local $io_arr_img2 = _cveInputOutputArrayFromMat($img2)

		_cvePolylines($io_arr_img2, $i_arr_dst_32, True, _cvScalar(0, 0, 255), 1, $CV_LINE_AA)

		; draw a circle at the center
		Local $moments = _cveMomentsCreate()
		_cveMoments($i_arr_dst_32, False, $moments)
		Local $cx = _cveMomentsGetM10($moments) / _cveMomentsGetM00($moments)
		Local $cy = _cveMomentsGetM01($moments) / _cveMomentsGetM00($moments)
		_cveCircleMat($img2, _cvPoint($cx, $cy), 5, _cvScalar(255, 255, 255), $CV_FILLED)
		_cveMomentsRelease($moments)

		_cveInputOutputArrayRelease($io_arr_img2)
		_cveInputArrayRelease($i_arr_dst_32)
		_VectorOfPointRelease($dst_int32)

		_cveOutputArrayRelease($o_arr_dst)
		_cveInputArrayRelease($i_arr_pts)
		_VectorOfPointFRelease($dst)
		_VectorOfPointFRelease($pts)
	EndIf

	_cveInputArrayRelease($i_arr_dst)
	_cveInputArrayRelease($i_arr_src)
	_cveOutputArrayRelease($o_arr_M)
	_cveInputArrayRelease($i_arr_M)
	_cveMatRelease($M)
	_VectorOfPointFRelease($dst_pts)
	_VectorOfPointFRelease($src_pts)
EndIf

;; draw match lines
Local $iEnd = _Min(20, UBound($dmatches))
Local $good_matches[$iEnd]
For $i = 0 To $iEnd - 1
	$good_matches[$i] = $dmatches[$i]
Next

Local $res = _cveMatCreate()
Local $matchesMask = _VectorOfByteCreate()
_drawMatchedFeatures1Mat($img1, $kpts1, $img2, $kpts2, $good_matches, $res, _cvScalarAll(-1), _cvScalarAll(-1), $matchesMask, $CV_DRAW_MATCHES_FLAGS_NOT_DRAW_SINGLE_POINTS)
_VectorOfByteRelease($matchesMask)

;-- Show detected matches
_cveImshowMat("found", $img2)
_cveImshowMat("Good Matches & Object detection", $res) ;
_cveWaitKey()
_cveDestroyAllWindows()

_cveMatRelease($res)
_VectorOfDMatchRelease($matches)
_cveBFMatcherRelease($bf_matcher)
_cveMatRelease($descs2)
_cveMatRelease($descs1)
_VectorOfKeyPointRelease($kpts2)
_VectorOfKeyPointRelease($kpts1)

_cveMatRelease($gray2)
_cveMatRelease($gray1)
_cveMatRelease($img2)
_cveMatRelease($img1)

Call($destructor, $tSharedPtr)

_Opencv_DLLClose()

Func _CompareDistance($a, $b)
	Return $a.distance - $b.distance
EndFunc   ;==>_CompareDistance

Func __ArrayQuickSort(ByRef $aArray, Const ByRef $sCompare, Const ByRef $iStart, Const ByRef $iEnd)
	If $iEnd <= $iStart Then Return

	Local $vTmp

	; InsertionSort (faster for smaller segments)
	If ($iEnd - $iStart) < 15 Then
		Local $vCur
		For $i = $iStart + 1 To $iEnd
			$vTmp = $aArray[$i]

			For $j = $i - 1 To $iStart Step -1
				If Call($sCompare, $vTmp, $aArray[$j]) >= 0 Then ExitLoop
				$aArray[$j + 1] = $aArray[$j]
			Next

			$aArray[$j + 1] = $vTmp
		Next
		Return
	EndIf

	; QuickSort
	Local $L = $iStart, $R = $iEnd, $vPivot = $aArray[Int(($iStart + $iEnd) / 2)]
	Do
		While Call($sCompare, $aArray[$L], $vPivot) < 0
			$L += 1
		WEnd
		While Call($sCompare, $aArray[$R], $vPivot) > 0
			$R -= 1
		WEnd

		; Swap
		If $L <= $R Then
			If $L <> $R Then
				$vTmp = $aArray[$L]
				$aArray[$L] = $aArray[$R]
				$aArray[$R] = $vTmp
			EndIf
			$L += 1
			$R -= 1
		EndIf
	Until $L > $R

	__ArrayQuickSort($aArray, $sCompare, $iStart, $R)
	__ArrayQuickSort($aArray, $sCompare, $L, $iEnd)
EndFunc   ;==>__ArrayQuickSort