using MathNet.Numerics.LinearAlgebra;
using System.Drawing;

namespace Cis.Application.Core;

public abstract class MarkSearcherBase
{
	#region Attr

	/// <summary>
	///  当前相机参数信息
	/// </summary>
	protected CameraCalcInfo CameraCalcInfo { get; set; }

	protected List<MarkLabelCalcInfo> MarkLabelInfoList { get; set; } = new();

	protected MatrixBuilder<double> MBuilder { get; set; } = Matrix<double>.Build;

	/// <summary>
	///  世界坐标转化为相机坐标矩阵
	/// </summary>
	protected Matrix<double> World2CameraMatrix { get; set; }

	#endregion Attr

	public MarkSearcherBase(CameraCalcInfo cameraCalcInfo)
	{
		InitCameraCalcInfoRelated(cameraCalcInfo);
	}

	protected void InitCameraCalcInfoRelated(CameraCalcInfo cameraCalcInfo)
	{
		CameraCalcInfo = cameraCalcInfo;
		CalcSensor();
		World2CameraMatrix = ConvertWorldToCamera(cameraCalcInfo);
	}

	#region Calc

	/// <summary>
	///  计算标签位置过程
	/// </summary>
	/// <returns></returns>
	public void Calc(CameraCalcInfo cameraCalcInfo)
	{
		//判断
		if (IsCameraRotate(cameraCalcInfo))
			InitCameraCalcInfoRelated(cameraCalcInfo);

		if (World2CameraMatrix == null || MarkLabelInfoList.Count == 0)
			return;

		for (int index = 0; index < MarkLabelInfoList.Count; index++)
		{
			MarkLabelCalcInfo label = MarkLabelInfoList[index];
			Matrix<double> labelC2WMatrix = ConvertCameraToWorld(label);
			Matrix<double> labelPointMatrix = MBuilder.DenseOfArray(new double[,]
			{
				{ label.CanvasWidth / CameraCalcInfo.ImageWidth - 0.5 },
				{ label.CanvasHeight / CameraCalcInfo.ImageHeight - 0.5 },
				{ 1 }
			});
			Matrix<double> lResult = labelC2WMatrix.Multiply(labelPointMatrix);
			Matrix<double> pResult = World2CameraMatrix.Multiply(lResult);

			double x = pResult[0, 0] / pResult[2, 0] + 0.5;
			double y = pResult[1, 0] / pResult[2, 0] + 0.5;
			MarkLabelCalcResult labelCalcResult = new();
			if (x > 0.99 || x < 0.01 || y > 0.99 || y < 0.01 || pResult[2, 0] < 0)
			{
				labelCalcResult.InFlag = false;
			}
			else
			{
				labelCalcResult.InFlag = true;
				labelCalcResult.CanvasLeft = x * CameraCalcInfo.ImageWidth;
				labelCalcResult.CanvasTop = y * CameraCalcInfo.ImageHeight;
			}
		}
	}

	/// <summary>
	///  判断相机是否进行了转动，转动了则需要重新计算世界坐标到相机坐标的转换矩阵
	/// </summary>
	/// <param name="newInfo"></param>
	/// <returns></returns>
	protected bool IsCameraRotate(CameraCalcInfo newInfo)
	{
		return CameraCalcInfo == null ||
			CameraCalcInfo.PtzInfo.Pan != newInfo.PtzInfo.Pan ||
			CameraCalcInfo.PtzInfo.Tilt != newInfo.PtzInfo.Tilt ||
			CameraCalcInfo.PtzInfo.Zoom != newInfo.PtzInfo.Zoom;
	}

	/// <summary>
	///  获取将世界坐标系中的点转化为相机坐标系中的点的转换矩阵
	/// </summary>
	/// <param name="cameraCalcInfo"></param>
	/// <returns></returns>
	protected Matrix<double> ConvertWorldToCamera(CameraCalcInfo cameraCalcInfo)
	{
		double panAngle = ConvertPanPosToAngle(cameraCalcInfo.PtzInfo.Pan);
		double tiltAngle = ConvertTiltPosToAngle(cameraCalcInfo.PtzInfo.Tilt);
		PointF pointF = GetFOfMatrixByZoomPos(cameraCalcInfo.PtzInfo.Zoom);
		double sinPan = Math.Sin(panAngle);
		double cosPan = Math.Cos(panAngle);
		double sinTilt = Math.Sin(tiltAngle);
		double cosTilt = Math.Cos(tiltAngle);

		Matrix<double> fMatrix = MBuilder.DenseOfArray(new double[,]
		{
			{ pointF.X, 0, 0 },
			{ 0, pointF.Y, 0 },
			{ 0, 0, 1 },
		});

		Matrix<double> rotateTiltMatrix = MBuilder.DenseOfArray(new double[,]
		{
			{ 1, 0, 0 },
			{ 0, cosTilt, sinTilt },
			{ 0, -sinTilt, cosTilt },
		});

		Matrix<double> midResult = fMatrix.Multiply(rotateTiltMatrix);

		Matrix<double> rotatePanMatrix = MBuilder.DenseOfArray(new double[,]
		{
			{ cosPan, 0, sinPan },
			{ 0, 1, 0 },
			{ -sinPan, 0, cosPan },
		});

		return midResult.Multiply(rotatePanMatrix);
	}

	/// <summary>
	///  获取将相机坐标系中的点转化为世界坐标系中的点的转换矩阵
	/// </summary>
	/// <param name="labelCalcInfo"></param>
	/// <returns></returns>
	protected Matrix<double> ConvertCameraToWorld(MarkLabelCalcInfo labelCalcInfo)
	{
		double panAngle = ConvertPanPosToAngle(labelCalcInfo.PtzInfo.Pan);
		double tiltAngle = ConvertTiltPosToAngle(labelCalcInfo.PtzInfo.Tilt);
		PointF pointF = GetFOfMatrixByZoomPos(labelCalcInfo.PtzInfo.Zoom);
		double sinPan = Math.Sin(panAngle);
		double cosPan = Math.Cos(panAngle);
		double sinTilt = Math.Sin(tiltAngle);
		double cosTilt = Math.Cos(tiltAngle);

		Matrix<double> rotatePanMatrix = MBuilder.DenseOfArray(new double[,] {
			{ cosPan, 0, -sinPan },
			{ 0, 1, 0 },
			{ sinPan, 0, cosPan }
		});

		Matrix<double> rotateTiltMatrix = MBuilder.DenseOfArray(new double[,] {
			{ 1, 0, 0 },
			{ 0, cosTilt, -sinTilt },
			{ 0, sinTilt, cosTilt }
		});

		Matrix<double> midResult = rotatePanMatrix.Multiply(rotateTiltMatrix);

		Matrix<double> fMatrix = MBuilder.DenseOfArray(new double[,] {
			{ (1 / pointF.X), 0, 0 },
			{ 0, (1 / pointF.Y), 0 },
			{ 0, 0, 1 }
		});

		return midResult.Multiply(fMatrix);
	}

	/// <summary>
	///  此方法计算在球机zoom值最小的情况下成像矩阵中的 f 本质为获取像元大小
	///  尝试方案1：通过计算的方式来获取
	///  尝试方案2：通过张友定相机标定的方法来生成成像矩阵中的 f
	/// </summary>
	protected virtual void CalcSensor()
	{
	}

	#endregion Calc

	#region Util

	/// <summary>
	///  将Pan值转化为角度
	/// </summary>
	/// <returns></returns>
	protected abstract double ConvertPanPosToAngle(double panPos);

	/// <summary>
	///  将Tilt转化为角度
	/// </summary>
	/// <returns></returns>
	protected abstract double ConvertTiltPosToAngle(double tiltPos, double tiltMinPos = 0);

	/// <summary>
	///  根据当前zoom值获取相机矩阵参数
	/// </summary>
	/// <param name="zoomPos"></param>
	/// <returns></returns>
	protected abstract PointF GetFOfMatrixByZoomPos(double zoomPos);

	#endregion Util
}