//go:build !gocv_specific_modules || (gocv_specific_modules && gocv_calib3d)

package gocv

import (
	"fmt"
	"image"
	"image/color"
	"math"
	"testing"
)

func TestFisheyeCalibrate(t *testing.T) {
	img := IMRead("images/chessboard_4x6_distort.png", IMReadGrayScale)
	if img.Empty() {
		t.Error("Invalid read of chessboard image")
		return
	}
	defer img.Close()

	corners := NewMat()
	defer corners.Close()

	size := image.Pt(4, 6)
	found := FindChessboardCorners(img, size, &corners, 0)
	if !found {
		t.Error("chessboard pattern not found")
		return
	}
	if corners.Empty() {
		t.Error("chessboard pattern not found")
		return
	}

	imagePoints := NewPoint2fVectorFromMat(corners)
	defer imagePoints.Close()

	objectPoints := NewPoint3fVector()
	defer objectPoints.Close()

	for j := 0; j < size.Y; j++ {
		for i := 0; i < size.X; i++ {
			objectPoints.Append(Point3f{
				X: float32(100 * i),
				Y: float32(100 * j),
				Z: 0,
			})
		}
	}

	k := NewMat()
	defer k.Close()
	d := NewMat()
	defer d.Close()
	rvecs := NewMat()
	defer rvecs.Close()
	tvecs := NewMat()
	defer tvecs.Close()

	objectPointsVector := NewPoints3fVector()
	objectPointsVector.Append(objectPoints)
	defer objectPointsVector.Close()

	imagePointsVector := NewPoints2fVector()
	imagePointsVector.Append(imagePoints)
	defer imagePointsVector.Close()

	FisheyeCalibrate(
		objectPointsVector, imagePointsVector, image.Pt(img.Cols(), img.Rows()),
		&k, &d, &rvecs, &tvecs, 0,
	)

	if rvecs.Empty() {
		t.Error("rvecs result is empty")
		return
	}

	if tvecs.Empty() {
		t.Error("tvecs result is empty")
		return
	}
}

func TestFisheyeDistortPoints(t *testing.T) {
	k := NewMatWithSize(3, 3, MatTypeCV64F)
	defer k.Close()

	k.SetDoubleAt(0, 0, 1094.7249578198823)
	k.SetDoubleAt(0, 1, 0)
	k.SetDoubleAt(0, 2, 959.4907612030962)

	k.SetDoubleAt(1, 0, 0)
	k.SetDoubleAt(1, 1, 1094.9945708128778)
	k.SetDoubleAt(1, 2, 536.4566143451868)

	k.SetDoubleAt(2, 0, 0)
	k.SetDoubleAt(2, 1, 0)
	k.SetDoubleAt(2, 2, 1)

	d := NewMatWithSize(1, 4, MatTypeCV64F)
	defer d.Close()

	d.SetDoubleAt(0, 0, -0.05207412392075069)
	d.SetDoubleAt(0, 1, -0.089168300192224)
	d.SetDoubleAt(0, 2, 0.10465607695792184)
	d.SetDoubleAt(0, 3, -0.045693446831115585)

	// transform 3 points in one go (X and Y values of points go in each channel)
	src := NewMatWithSize(3, 1, MatTypeCV64FC2)
	defer src.Close()

	dst := NewMat()
	defer dst.Close()

	// This camera matrix is 1920x1080. Points where x < 960 and y < 540 should move toward the top left (x and y get smaller)
	// The centre point should be mostly unchanged
	// Points where x > 960 and y > 540 should move toward the bottom right (x and y get bigger)

	// The index being used for col here is actually the channel (i.e. the point's x/y dimensions)
	// (since there's only 1 column so the formula: (colNumber * numChannels + channelNumber) reduces to
	// (0 * 2) + channelNumber
	// so col = 0 is the x coordinate and col = 1 is the y coordinate

	src.SetDoubleAt(0, 0, 480)
	src.SetDoubleAt(0, 1, 270)

	src.SetDoubleAt(1, 0, 960)
	src.SetDoubleAt(1, 1, 540)

	src.SetDoubleAt(2, 0, 1440)
	src.SetDoubleAt(2, 1, 810)

	FisheyeDistortPoints(src, &dst, k, d)

	if dst.Empty() {
		t.Error("final image is empty")
		return
	}
}

func TestFisheyeUndistorImage(t *testing.T) {
	img := IMRead("images/fisheye_sample.jpg", IMReadUnchanged)
	if img.Empty() {
		t.Error("Invalid read of Mat test")
		return
	}
	defer img.Close()

	dest := NewMat()
	defer dest.Close()

	k := NewMatWithSize(3, 3, MatTypeCV64F)
	defer k.Close()

	k.SetDoubleAt(0, 0, 689.21)
	k.SetDoubleAt(0, 1, 0)
	k.SetDoubleAt(0, 2, 1295.56)

	k.SetDoubleAt(1, 0, 0)
	k.SetDoubleAt(1, 1, 690.48)
	k.SetDoubleAt(1, 2, 942.17)

	k.SetDoubleAt(2, 0, 0)
	k.SetDoubleAt(2, 1, 0)
	k.SetDoubleAt(2, 2, 1)

	d := NewMatWithSize(1, 4, MatTypeCV64F)
	defer d.Close()

	d.SetDoubleAt(0, 0, 0)
	d.SetDoubleAt(0, 1, 0)
	d.SetDoubleAt(0, 2, 0)
	d.SetDoubleAt(0, 3, 0)

	FisheyeUndistortImage(img, &dest, k, d)

	if dest.Empty() {
		t.Error("final image is empty")
		return
	}
}

func TestFisheyeUndistorImageWithParams(t *testing.T) {
	img := IMRead("images/fisheye_sample.jpg", IMReadUnchanged)
	if img.Empty() {
		t.Error("Invalid read of Mat test")
		return
	}
	defer img.Close()

	dest := NewMat()
	defer dest.Close()

	k := NewMatWithSize(3, 3, MatTypeCV64F)
	defer k.Close()

	k.SetDoubleAt(0, 0, 689.21)
	k.SetDoubleAt(0, 1, 0)
	k.SetDoubleAt(0, 2, 1295.56)

	k.SetDoubleAt(1, 0, 0)
	k.SetDoubleAt(1, 1, 690.48)
	k.SetDoubleAt(1, 2, 942.17)

	k.SetDoubleAt(2, 0, 0)
	k.SetDoubleAt(2, 1, 0)
	k.SetDoubleAt(2, 2, 1)

	d := NewMatWithSize(1, 4, MatTypeCV64F)
	defer d.Close()

	d.SetDoubleAt(0, 0, 0)
	d.SetDoubleAt(0, 1, 0)
	d.SetDoubleAt(0, 2, 0)
	d.SetDoubleAt(0, 3, 0)

	knew := NewMat()
	defer knew.Close()

	k.CopyTo(&knew)

	knew.SetDoubleAt(0, 0, 0.4*k.GetDoubleAt(0, 0))
	knew.SetDoubleAt(1, 1, 0.4*k.GetDoubleAt(1, 1))

	size := image.Point{dest.Rows(), dest.Cols()}
	FisheyeUndistortImageWithParams(img, &dest, k, d, knew, size)

	if dest.Empty() {
		t.Error("final image is empty")
		return
	}
}

func TestInitUndistortRectifyMap(t *testing.T) {
	img := IMRead("images/distortion.jpg", IMReadUnchanged)
	if img.Empty() {
		t.Error("Invalid read of Mat test")
		return
	}
	defer img.Close()

	dest := NewMat()
	defer dest.Close()

	k := NewMatWithSize(3, 3, MatTypeCV64F)
	defer k.Close()

	k.SetDoubleAt(0, 0, 842.0261028)
	k.SetDoubleAt(0, 1, 0)
	k.SetDoubleAt(0, 2, 667.7569792)

	k.SetDoubleAt(1, 0, 0)
	k.SetDoubleAt(1, 1, 707.3668897)
	k.SetDoubleAt(1, 2, 385.56476464)

	k.SetDoubleAt(2, 0, 0)
	k.SetDoubleAt(2, 1, 0)
	k.SetDoubleAt(2, 2, 1)

	d := NewMatWithSize(1, 5, MatTypeCV64F)
	defer d.Close()

	d.SetDoubleAt(0, 0, -3.65584802e-01)
	d.SetDoubleAt(0, 1, 1.41555815e-01)
	d.SetDoubleAt(0, 2, -2.62985819e-03)
	d.SetDoubleAt(0, 3, 2.05841873e-04)
	d.SetDoubleAt(0, 4, -2.35021914e-02)

	newC, roi := GetOptimalNewCameraMatrixWithParams(k, d, image.Point{X: img.Cols(), Y: img.Rows()}, (float64)(1), image.Point{X: img.Cols(), Y: img.Rows()}, false)
	if newC.Empty() {
		t.Error("final image is empty")
		return
	}
	fmt.Printf("roi:%+v\n", roi)
	defer newC.Close()
	r := NewMat()
	defer r.Close()
	mapx := NewMat()
	defer mapx.Close()
	mapy := NewMat()
	defer mapy.Close()

	InitUndistortRectifyMap(k, d, r, newC, image.Point{X: img.Cols(), Y: img.Rows()}, 5, mapx, mapy)

	Remap(img, &dest, &mapx, &mapy, InterpolationDefault, BorderConstant, color.RGBA{0, 0, 0, 0})
	flg := IMWrite("images/distortion-correct.jpg", dest)
	if !flg {
		t.Error("IMWrite failed")
	}
}

func TestUndistort(t *testing.T) {
	img := IMRead("images/distortion.jpg", IMReadUnchanged)
	if img.Empty() {
		t.Error("Invalid read of Mat test")
		return
	}
	defer img.Close()

	dest := img.Clone()
	defer dest.Close()

	k := NewMatWithSize(3, 3, MatTypeCV64F)
	defer k.Close()

	k.SetDoubleAt(0, 0, 689.21)
	k.SetDoubleAt(0, 1, 0)
	k.SetDoubleAt(0, 2, 1295.56)

	k.SetDoubleAt(1, 0, 0)
	k.SetDoubleAt(1, 1, 690.48)
	k.SetDoubleAt(1, 2, 942.17)

	k.SetDoubleAt(2, 0, 0)
	k.SetDoubleAt(2, 1, 0)
	k.SetDoubleAt(2, 2, 1)

	d := NewMatWithSize(1, 4, MatTypeCV64F)
	defer d.Close()

	d.SetDoubleAt(0, 0, 0)
	d.SetDoubleAt(0, 1, 0)
	d.SetDoubleAt(0, 2, 0)
	d.SetDoubleAt(0, 3, 0)

	knew := NewMat()
	defer knew.Close()

	k.CopyTo(&knew)

	knew.SetDoubleAt(0, 0, 0.5*k.GetDoubleAt(0, 0))
	knew.SetDoubleAt(1, 1, 0.5*k.GetDoubleAt(1, 1))

	Undistort(img, &dest, k, d, knew)

	if dest.Empty() {
		t.Error("final image is empty")
		return
	}
}

func TestUndistortPoint(t *testing.T) {
	k := NewMatWithSize(3, 3, MatTypeCV64F)
	defer k.Close()

	k.SetDoubleAt(0, 0, 1094.7249578198823)
	k.SetDoubleAt(0, 1, 0)
	k.SetDoubleAt(0, 2, 959.4907612030962)

	k.SetDoubleAt(1, 0, 0)
	k.SetDoubleAt(1, 1, 1094.9945708128778)
	k.SetDoubleAt(1, 2, 536.4566143451868)

	k.SetDoubleAt(2, 0, 0)
	k.SetDoubleAt(2, 1, 0)
	k.SetDoubleAt(2, 2, 1)

	d := NewMatWithSize(1, 4, MatTypeCV64F)
	defer d.Close()

	d.SetDoubleAt(0, 0, -0.05207412392075069)
	d.SetDoubleAt(0, 1, -0.089168300192224)
	d.SetDoubleAt(0, 2, 0.10465607695792184)
	d.SetDoubleAt(0, 3, -0.045693446831115585)

	r := NewMat()
	defer r.Close()

	// transform 3 points in one go
	src := NewMatWithSize(3, 1, MatTypeCV64FC2)
	defer src.Close()
	dst := NewMatWithSize(3, 1, MatTypeCV64FC2)
	defer dst.Close()

	// This camera matrix is 1920x1080. Points where x < 960 and y < 540 should move toward the top left (x and y get smaller)
	// The centre point should be mostly unchanged
	// Points where x > 960 and y > 540 should move toward the bottom right (x and y get bigger)

	// The index being used for col here is actually the channel (i.e. the point's x/y dimensions)
	// (since there's only 1 column so the formula: (colNumber * numChannels + channelNumber) reduces to
	// (0 * 2) + channelNumber
	// so col = 0 is the x coordinate and col = 1 is the y coordinate

	src.SetDoubleAt(0, 0, 480)
	src.SetDoubleAt(0, 1, 270)

	src.SetDoubleAt(1, 0, 960)
	src.SetDoubleAt(1, 1, 540)

	src.SetDoubleAt(2, 0, 1920)
	src.SetDoubleAt(2, 1, 1080)

	UndistortPoints(src, &dst, k, d, r, k)

	if dst.GetDoubleAt(0, 0) >= 480 || dst.GetDoubleAt(0, 1) >= 270 {
		t.Error("undistortion expected top left point to move further up and left")
		return
	}

	if math.Round(dst.GetDoubleAt(1, 0)) != 960 || math.Round(dst.GetDoubleAt(1, 1)) != 540 {
		t.Error("undistortion expected centre point to be nearly unchanged")
		return
	}

	if dst.GetDoubleAt(2, 0) != 1920 || dst.GetDoubleAt(2, 1) != 1080 {
		t.Error("undistortion expected bottom right corner to be unchanged")
		return
	}

}

func TestFisheyeUndistortPoint(t *testing.T) {
	k := NewMatWithSize(3, 3, MatTypeCV64F)
	defer k.Close()

	k.SetDoubleAt(0, 0, 1094.7249578198823)
	k.SetDoubleAt(0, 1, 0)
	k.SetDoubleAt(0, 2, 959.4907612030962)

	k.SetDoubleAt(1, 0, 0)
	k.SetDoubleAt(1, 1, 1094.9945708128778)
	k.SetDoubleAt(1, 2, 536.4566143451868)

	k.SetDoubleAt(2, 0, 0)
	k.SetDoubleAt(2, 1, 0)
	k.SetDoubleAt(2, 2, 1)

	d := NewMatWithSize(1, 4, MatTypeCV64F)
	defer d.Close()

	d.SetDoubleAt(0, 0, -0.05207412392075069)
	d.SetDoubleAt(0, 1, -0.089168300192224)
	d.SetDoubleAt(0, 2, 0.10465607695792184)
	d.SetDoubleAt(0, 3, -0.045693446831115585)

	r := NewMat()
	defer r.Close()

	// transform 3 points in one go (X and Y values of points go in each channel)
	src := NewMatWithSize(3, 1, MatTypeCV64FC2)
	defer src.Close()
	dst := NewMatWithSize(3, 1, MatTypeCV64FC2)
	defer dst.Close()

	// This camera matrix is 1920x1080. Points where x < 960 and y < 540 should move toward the top left (x and y get smaller)
	// The centre point should be mostly unchanged
	// Points where x > 960 and y > 540 should move toward the bottom right (x and y get bigger)

	// The index being used for col here is actually the channel (i.e. the point's x/y dimensions)
	// (since there's only 1 column so the formula: (colNumber * numChannels + channelNumber) reduces to
	// (0 * 2) + channelNumber
	// so col = 0 is the x coordinate and col = 1 is the y coordinate

	src.SetDoubleAt(0, 0, 480)
	src.SetDoubleAt(0, 1, 270)

	src.SetDoubleAt(1, 0, 960)
	src.SetDoubleAt(1, 1, 540)

	src.SetDoubleAt(2, 0, 1440)
	src.SetDoubleAt(2, 1, 810)

	kNew := NewMat()
	defer kNew.Close()

	k.CopyTo(&kNew)

	kNew.SetDoubleAt(0, 0, 0.4*k.GetDoubleAt(0, 0))
	kNew.SetDoubleAt(1, 1, 0.4*k.GetDoubleAt(1, 1))

	imgSize := image.Point{X: 1920, Y: 1080}

	EstimateNewCameraMatrixForUndistortRectify(k, d, imgSize, r, &kNew, 1, imgSize, 1)

	FisheyeUndistortPoints(src, &dst, k, d, r, kNew)

	if dst.GetDoubleAt(0, 0) == 0 {
		t.Error("expected destination Mat to be populated")
	}
}

func TestCheckChessboard(t *testing.T) {
	img := IMRead("images/chessboard_4x6.png", IMReadGrayScale)
	if img.Empty() {
		t.Error("Invalid read of chessboard image")
		return
	}
	defer img.Close()

	if !CheckChessboard(img, image.Point{X: 4, Y: 6}) {
		t.Error("chessboard pattern not found")
		return
	}
}

func TestFindAndDrawChessboard(t *testing.T) {
	img := IMRead("images/chessboard_4x6.png", IMReadUnchanged)
	if img.Empty() {
		t.Error("Invalid read of chessboard image")
		return
	}
	defer img.Close()

	corners := NewMat()
	defer corners.Close()

	found := FindChessboardCorners(img, image.Point{X: 4, Y: 6}, &corners, 0)
	if found == false {
		t.Error("chessboard pattern not found")
		return
	}
	if corners.Empty() {
		t.Error("chessboard pattern not found")
		return
	}

	img2 := NewMatWithSize(150, 150, MatTypeCV8U)
	defer img2.Close()

	DrawChessboardCorners(&img2, image.Pt(4, 6), corners, true)
	if img2.Empty() {
		t.Error("Error in DrawChessboardCorners test")
	}
}

func TestFindAndDrawChessboardSB(t *testing.T) {
	img := IMRead("images/chessboard_4x6.png", IMReadUnchanged)
	if img.Empty() {
		t.Error("Invalid read of chessboard image")
		return
	}
	defer img.Close()

	corners := NewMat()
	defer corners.Close()

	found := FindChessboardCornersSB(img, image.Point{X: 4, Y: 6}, &corners, 0)
	if found == false {
		t.Error("chessboard pattern not found")
		return
	}
	if corners.Empty() {
		t.Error("chessboard pattern not found")
		return
	}

	img2 := NewMatWithSize(150, 150, MatTypeCV8U)
	defer img2.Close()

	DrawChessboardCorners(&img2, image.Pt(4, 6), corners, true)
	if img2.Empty() {
		t.Error("Error in DrawChessboardCorners test")
	}
}

func TestFindChessboardCornersSBWithMeta(t *testing.T) {
	img := IMRead("images/chessboard_4x6.png", IMReadUnchanged)
	if img.Empty() {
		t.Error("Invalid read of chessboard image")
		return
	}
	defer img.Close()

	corners := NewMat()
	defer corners.Close()

	meta := NewMat()
	defer meta.Close()

	found := FindChessboardCornersSBWithMeta(img, image.Point{X: 4, Y: 6}, &corners, 0, &meta)
	if found == false {
		t.Error("chessboard pattern not found")
		return
	}
	if corners.Empty() {
		t.Error("chessboard pattern not found")
		return
	}

	img2 := NewMatWithSize(150, 150, MatTypeCV8U)
	defer img2.Close()

	DrawChessboardCorners(&img2, image.Pt(4, 6), corners, true)
	if img2.Empty() {
		t.Error("Error in DrawChessboardCorners test")
	}
}

func TestCalibrateCamera(t *testing.T) {
	img := IMRead("images/chessboard_4x6_distort.png", IMReadGrayScale)
	if img.Empty() {
		t.Error("Invalid read of chessboard image")
		return
	}
	defer img.Close()

	corners := NewMat()
	defer corners.Close()

	size := image.Pt(4, 6)
	found := FindChessboardCorners(img, size, &corners, 0)
	if !found {
		t.Error("chessboard pattern not found")
		return
	}
	if corners.Empty() {
		t.Error("chessboard pattern not found")
		return
	}

	imagePoints := NewPoint2fVectorFromMat(corners)
	defer imagePoints.Close()

	objectPoints := NewPoint3fVector()
	defer objectPoints.Close()

	for j := 0; j < size.Y; j++ {
		for i := 0; i < size.X; i++ {
			objectPoints.Append(Point3f{
				X: float32(100 * i),
				Y: float32(100 * j),
				Z: 0,
			})
		}
	}

	cameraMatrix := NewMat()
	defer cameraMatrix.Close()
	distCoeffs := NewMat()
	defer distCoeffs.Close()
	rvecs := NewMat()
	defer rvecs.Close()
	tvecs := NewMat()
	defer tvecs.Close()

	objectPointsVector := NewPoints3fVector()
	objectPointsVector.Append(objectPoints)
	defer objectPointsVector.Close()

	imagePointsVector := NewPoints2fVector()
	imagePointsVector.Append(imagePoints)
	defer imagePointsVector.Close()

	CalibrateCamera(
		objectPointsVector, imagePointsVector, image.Pt(img.Cols(), img.Rows()),
		&cameraMatrix, &distCoeffs, &rvecs, &tvecs, 0,
	)

	dest := NewMat()
	defer dest.Close()
	Undistort(img, &dest, cameraMatrix, distCoeffs, cameraMatrix)

	target := IMRead("images/chessboard_4x6_distort_correct.png", IMReadGrayScale)
	defer target.Close()

	xor := NewMat()
	defer xor.Close()

	// The method for compare is ugly : different pix number < 0.5%
	BitwiseXor(dest, target, &xor)
	differentPixelsNumber := xor.Sum().Val1
	maxDifferentPixelsNumber := float64(img.Cols()*img.Rows()) * 0.005
	if differentPixelsNumber > maxDifferentPixelsNumber {
		t.Error("the undisorted image not equal the target one:", differentPixelsNumber, "bigger than", maxDifferentPixelsNumber)
	}
}

func TestEstimateAffinePartial2D(t *testing.T) {
	src := []Point2f{
		{0, 0},
		{10, 5},
		{10, 10},
		{5, 10},
	}

	dst := []Point2f{
		{0, 0},
		{10, 0},
		{10, 10},
		{0, 10},
	}

	pvsrc := NewPoint2fVectorFromPoints(src)
	defer pvsrc.Close()

	pvdst := NewPoint2fVectorFromPoints(dst)
	defer pvdst.Close()

	m := EstimateAffinePartial2D(pvsrc, pvdst)
	defer m.Close()

	if m.Cols() != 3 {
		t.Errorf("TestEstimateAffinePartial2D(): unexpected cols = %v, want = %v", m.Cols(), 3)
	}
	if m.Rows() != 2 {
		t.Errorf("TestEstimateAffinePartial2D(): unexpected rows = %v, want = %v", m.Rows(), 2)
	}
}

func TestEstimateAffinePartial2DWithParams(t *testing.T) {
	src := []Point2f{
		{0, 0},
		{10, 5},
		{10, 10},
		{5, 10},
	}

	dst := []Point2f{
		{0, 0},
		{10, 0},
		{10, 10},
		{0, 10},
	}

	pvsrc := NewPoint2fVectorFromPoints(src)
	defer pvsrc.Close()

	pvdst := NewPoint2fVectorFromPoints(dst)
	defer pvdst.Close()

	inliers := NewMat()
	defer inliers.Close()
	method := 8
	ransacProjThreshold := 3.0
	maxiters := uint(2000)
	confidence := 0.99
	refineIters := uint(10)

	m := EstimateAffinePartial2DWithParams(pvsrc, pvdst, inliers, method, ransacProjThreshold, maxiters, confidence, refineIters)
	defer m.Close()

	if m.Cols() != 3 {
		t.Errorf("TestEstimateAffinePartial2D(): unexpected cols = %v, want = %v", m.Cols(), 3)
	}
	if m.Rows() != 2 {
		t.Errorf("TestEstimateAffinePartial2D(): unexpected rows = %v, want = %v", m.Rows(), 2)
	}
}

func TestEstimateAffine2D(t *testing.T) {
	src := []Point2f{
		{0, 0},
		{10, 5},
		{10, 10},
		{5, 10},
	}

	dst := []Point2f{
		{0, 0},
		{10, 0},
		{10, 10},
		{0, 10},
	}

	pvsrc := NewPoint2fVectorFromPoints(src)
	defer pvsrc.Close()

	pvdst := NewPoint2fVectorFromPoints(dst)
	defer pvdst.Close()

	m := EstimateAffine2D(pvsrc, pvdst)
	defer m.Close()

	if m.Cols() != 3 {
		t.Errorf("TestEstimateAffine2D(): unexpected cols = %v, want = %v", m.Cols(), 3)
	}
	if m.Rows() != 2 {
		t.Errorf("TestEstimateAffine2D(): unexpected rows = %v, want = %v", m.Rows(), 2)
	}
}

func TestEstimateAffine2DWithParams(t *testing.T) {
	src := []Point2f{
		{0, 0},
		{10, 5},
		{10, 10},
		{5, 10},
	}

	dst := []Point2f{
		{0, 0},
		{10, 0},
		{10, 10},
		{0, 10},
	}

	pvsrc := NewPoint2fVectorFromPoints(src)
	defer pvsrc.Close()

	pvdst := NewPoint2fVectorFromPoints(dst)
	defer pvdst.Close()

	inliers := NewMat()
	defer inliers.Close()
	method := 8
	ransacProjThreshold := 3.0
	maxiters := uint(2000)
	confidence := 0.99
	refineIters := uint(10)

	m := EstimateAffine2DWithParams(pvsrc, pvdst, inliers, method, ransacProjThreshold, maxiters, confidence, refineIters)
	defer m.Close()

	if m.Cols() != 3 {
		t.Errorf("TestEstimateAffine2DWithParams(): unexpected cols = %v, want = %v", m.Cols(), 3)
	}
	if m.Rows() != 2 {
		t.Errorf("TestEstimateAffine2DWithParams(): unexpected rows = %v, want = %v", m.Rows(), 2)
	}
}

func TestTriangulatePoints(t *testing.T) {
	projMat1, projMat2 := NewMatWithSize(3, 4, MatTypeCV64F), NewMatWithSize(3, 4, MatTypeCV64F)
	defer projMat1.Close()
	defer projMat2.Close()
	projPoints1, projPoints2 := NewPoint2fVectorFromPoints([]Point2f{{Y: 1.0, X: 2.0}}), NewPoint2fVectorFromPoints([]Point2f{{Y: 3.0, X: 4.0}})
	defer projPoints1.Close()
	defer projPoints2.Close()
	homogeneous := NewMat()
	defer homogeneous.Close()
	TriangulatePoints(projMat1, projMat2, projPoints1, projPoints2, &homogeneous)
	if homogeneous.Empty() {
		t.Errorf("TriangulatePoints(): output homogeneous mat is empty")
	}
}

func TestConvertPointsFromHomogeneous(t *testing.T) {
	homogeneous := NewMatWithSize(1, 4, MatTypeCV32F)
	defer homogeneous.Close()
	homogeneous.SetFloatAt(0, 0, 1)
	homogeneous.SetFloatAt(0, 1, 2)
	homogeneous.SetFloatAt(0, 2, 4)
	homogeneous.SetFloatAt(0, 3, 2)
	euclidean := NewMat()
	defer euclidean.Close()
	ConvertPointsFromHomogeneous(homogeneous, &euclidean)
	if euclidean.Empty() {
		t.Fatalf("ConvertPointsFromHomogeneous(): output euclidean mat is empty")
	}
	ptsVector := NewPoint3fVectorFromMat(euclidean)
	defer ptsVector.Close()
	pts := ptsVector.ToPoints()
	if len(pts) != 1 {
		t.Fatalf("ConvertPointsFromHomogeneous(): euclidean mat converted to points is empty")
	}
	if pts[0].X != 0.5 {
		t.Errorf("ConvertPointsFromHomogeneous(): euclidean X - got %v, want %v", pts[0].X, 0.5)
	}
	if pts[0].Y != 1 {
		t.Errorf("ConvertPointsFromHomogeneous(): euclidean Y - got %v, want %v", pts[0].Y, 1)
	}
	if pts[0].Z != 2 {
		t.Errorf("ConvertPointsFromHomogeneous(): euclidean Z - got %v, want %v", pts[0].Z, 2)
	}
}

func TestRodrigues(t *testing.T) {
	k := NewMatWithSize(3, 3, MatTypeCV64F)
	defer k.Close()

	k.SetDoubleAt(0, 0, 689.21)
	k.SetDoubleAt(0, 1, 0)
	k.SetDoubleAt(0, 2, 1295.56)

	k.SetDoubleAt(1, 0, 0)
	k.SetDoubleAt(1, 1, 690.48)
	k.SetDoubleAt(1, 2, 942.17)

	k.SetDoubleAt(2, 0, 0)
	k.SetDoubleAt(2, 1, 0)
	k.SetDoubleAt(2, 2, 1)

	dest := NewMat()
	defer dest.Close()

	Rodrigues(k, &dest)

	if dest.Empty() {
		t.Error("final result is empty")
		return
	}
}

func TestSolvePnP(t *testing.T) {
	pts := []Point3f{
		{10.0, 10.0, 0.1},
		{10.0, 20.0, 1.0},
		{20.5, 21.5, 2.0},
		{10.0, 20.0, 1.0},
	}

	objectPointsVector := NewPoint3fVectorFromPoints(pts)
	defer objectPointsVector.Close()

	pts2 := []Point2f{
		{10.0, 10.0},
		{10.0, 20.0},
		{20.5, 21.5},
		{25.5, 30.5},
	}

	imagePointsVector := NewPoint2fVectorFromPoints(pts2)
	defer imagePointsVector.Close()

	cameraMatrix := Eye(3, 3, MatTypeCV64F)
	defer cameraMatrix.Close()
	distCoeffs := NewMat()
	defer distCoeffs.Close()
	rvecs := NewMat()
	defer rvecs.Close()
	tvecs := NewMat()
	defer tvecs.Close()

	SolvePnP(objectPointsVector, imagePointsVector, cameraMatrix, distCoeffs,
		&rvecs, &tvecs, false, 0)

	if rvecs.Empty() {
		t.Error("rvecs result is empty")
		return
	}

	if tvecs.Empty() {
		t.Error("tvecs result is empty")
		return
	}
}

func TestStereoRectify(t *testing.T) {
	cameraMatrix1 := NewMatWithSize(3, 3, MatTypeCV64F)
	defer cameraMatrix1.Close()
	cameraMatrix1.SetDoubleAt(0, 0, 1679.861998224759)
	cameraMatrix1.SetDoubleAt(0, 1, 0)
	cameraMatrix1.SetDoubleAt(0, 2, 1231.158426771668)
	cameraMatrix1.SetDoubleAt(1, 0, 0)
	cameraMatrix1.SetDoubleAt(1, 1, 1679.6751024982132)
	cameraMatrix1.SetDoubleAt(1, 2, 998.4768157307255)
	cameraMatrix1.SetDoubleAt(2, 0, 0)
	cameraMatrix1.SetDoubleAt(2, 1, 0)
	cameraMatrix1.SetDoubleAt(2, 2, 1)

	distCoeffs1 := NewMatWithSize(1, 5, MatTypeCV64F)
	defer distCoeffs1.Close()
	distCoeffs1.SetDoubleAt(0, 0, -0.13657891044008158)
	distCoeffs1.SetDoubleAt(0, 1, 0.08861314898314139)
	distCoeffs1.SetDoubleAt(0, 2, -0.0006100429198910993)
	distCoeffs1.SetDoubleAt(0, 3, -0.000146035714553745)
	distCoeffs1.SetDoubleAt(0, 4, -0.0223854079208295)

	cameraMatrix2 := NewMatWithSize(3, 3, MatTypeCV64F)
	defer cameraMatrix2.Close()
	cameraMatrix2.SetDoubleAt(0, 0, 1678.7797594660801)
	cameraMatrix2.SetDoubleAt(0, 1, 0)
	cameraMatrix2.SetDoubleAt(0, 2, 1233.9647439048256)
	cameraMatrix2.SetDoubleAt(1, 0, 0)
	cameraMatrix2.SetDoubleAt(1, 1, 1679.0176277206788)
	cameraMatrix2.SetDoubleAt(1, 2, 964.4565768594671)
	cameraMatrix2.SetDoubleAt(2, 0, 0)
	cameraMatrix2.SetDoubleAt(2, 1, 0)
	cameraMatrix2.SetDoubleAt(2, 2, 1)

	distCoeffs2 := NewMatWithSize(1, 5, MatTypeCV64F)
	defer distCoeffs2.Close()
	distCoeffs2.SetDoubleAt(0, 0, -0.13690489442431938)
	distCoeffs2.SetDoubleAt(0, 1, 0.08705821688253863)
	distCoeffs2.SetDoubleAt(0, 2, 0.001288895752441417)
	distCoeffs2.SetDoubleAt(0, 3, -5.508164903909865e-05)
	distCoeffs2.SetDoubleAt(0, 4, -0.02092107478701842)

	R := NewMatWithSize(3, 3, MatTypeCV64F)
	defer R.Close()
	R.SetDoubleAt(0, 0, 0.9978384271270464)
	R.SetDoubleAt(0, 1, 0.06567174227009016)
	R.SetDoubleAt(0, 2, -0.0023865489378896566)
	R.SetDoubleAt(1, 0, -0.06567842187628788)
	R.SetDoubleAt(1, 1, 0.9978368073955511)
	R.SetDoubleAt(1, 2, -0.002837376692327134)
	R.SetDoubleAt(2, 0, 0.0021950509020153912)
	R.SetDoubleAt(2, 1, 0.0029879882638097722)
	R.SetDoubleAt(2, 2, 0.9999931268152161)

	T := NewMatWithSize(3, 1, MatTypeCV64F)
	defer T.Close()
	T.SetDoubleAt(0, 0, -0.1600062730246643)
	T.SetDoubleAt(1, 0, 0.007804854680409705)
	T.SetDoubleAt(2, 0, 9.242933249524358e-05)

	R1 := NewMat()
	defer R1.Close()
	R2 := NewMat()
	defer R2.Close()
	P1 := NewMat()
	defer P1.Close()
	P2 := NewMat()
	defer P2.Close()
	Q := NewMat()
	defer Q.Close()

	imageSize := image.Point{X: 2448, Y: 2048}
	err := StereoRectify(cameraMatrix1, distCoeffs1, cameraMatrix2, distCoeffs2, imageSize, R, T, &R1, &R2, &P1, &P2, &Q, CalibFlagPinholeZeroDisparity)
	if err != nil {
		t.Errorf("StereoRectify failed: %v", err)
	}

	if R1.Empty() {
		t.Error("R1 result is empty")
	}
	if R2.Empty() {
		t.Error("R2 result is empty")
	}
	if P1.Empty() {
		t.Error("P1 result is empty")
	}
	if P2.Empty() {
		t.Error("P2 result is empty")
	}
	if Q.Empty() {
		t.Error("Q result is empty")
	}
}

func TestFindHomography(t *testing.T) {
	src := NewMatWithSize(4, 1, MatTypeCV64FC2)
	defer src.Close()
	target := NewMatWithSize(4, 1, MatTypeCV64FC2)
	defer target.Close()

	srcPoints := []Point2f{
		{193, 932},
		{191, 378},
		{1497, 183},
		{1889, 681},
	}
	targetPoints := []Point2f{
		{51.51206544281359, -0.10425475260813055},
		{51.51211051314331, -0.10437947532732306},
		{51.512222354139325, -0.10437679311830816},
		{51.51214828037607, -0.1042212249954444},
	}

	for i, point := range srcPoints {
		src.SetDoubleAt(i, 0, float64(point.X))
		src.SetDoubleAt(i, 1, float64(point.Y))
	}

	for i, point := range targetPoints {
		target.SetDoubleAt(i, 0, float64(point.X))
		target.SetDoubleAt(i, 1, float64(point.Y))
	}

	mask := NewMat()
	defer mask.Close()

	m := FindHomography(src, target, HomographyMethodAllPoints, 3, &mask, 2000, 0.995)
	defer m.Close()

	pvsrc := NewPoint2fVectorFromPoints(srcPoints)
	defer pvsrc.Close()

	pvdst := NewPoint2fVectorFromPoints(targetPoints)
	defer pvdst.Close()

	m2 := GetPerspectiveTransform2f(pvsrc, pvdst)
	defer m2.Close()

	for row := 0; row < 3; row++ {
		for col := 0; col < 3; col++ {
			if math.Abs(m.GetDoubleAt(row, col)-m2.GetDoubleAt(row, col)) > 0.002 {
				t.Errorf("expected little difference between GetPerspectiveTransform2f and FindHomography results, got %f for row %d col %d", math.Abs(m.GetDoubleAt(row, col)-m2.GetDoubleAt(row, col)), row, col)
			}
		}
	}
}