using OpenCvSharp;
using System;
using System.Collections.Generic;
using System.Diagnostics;
using System.Drawing;
using System.Drawing.Imaging;
using System.Text;
using System.Windows.Forms;

namespace OpenCvSharp_Demo
{
    public partial class Form1 : Form
    {
        public Form1()
        {
            InitializeComponent();
        }

        string fileFilter = "*.*|*.bmp;*.jpg;*.jpeg;*.tiff;*.tiff;*.png";
        string startupPath;
        string image_path;

        Stopwatch stopwatch = new Stopwatch();

        Mat image;
        Mat result_image;

        //棋盘格的宽度和高度
        int BoardSize_Width = 9;
        int BoardSize_Height = 6;
        OpenCvSharp.Size BoardSize;

        //每个方格的宽度
        private  int SquareSize = 50;
        private  int winSize = 11;

        StringBuilder sb=new StringBuilder();

        private void Form1_Load(object sender, EventArgs e)
        {
            startupPath = System.Windows.Forms.Application.StartupPath;

            BoardSize = new OpenCvSharp.Size(BoardSize_Width, BoardSize_Height);

            image_path = "1.jpg";
            pictureBox1.Image = new Bitmap(image_path);
            image = new Mat(image_path);
        }

        private void button1_Click(object sender, EventArgs e)
        {
            OpenFileDialog ofd = new OpenFileDialog();
            ofd.Filter = fileFilter;
            if (ofd.ShowDialog() != DialogResult.OK) return;

            pictureBox1.Image = null;
            pictureBox2.Image = null;
            textBox1.Text = "";

            image_path = ofd.FileName;
            pictureBox1.Image = new Bitmap(image_path);
            image = new Mat(image_path);
        }

        private void button2_Click(object sender, EventArgs e)
        {
            stopwatch.Restart();
            sb.Clear();

            result_image = image.Clone();

            // 存储每个图像的棋盘角点
            List<Point2f[]> imagesPoints = new List<Point2f[]>();

            // 相机内参矩阵和畸变系数
            Mat cameraMatrix = new Mat(), distCoeffs = new Mat();

            // 图像的尺寸
            OpenCvSharp.Size imageSize = new OpenCvSharp.Size();
            bool found = false;

            // 读取图像
            Mat view = new Mat(image_path);
            Mat p = null;

            if (!view.Empty())
            {
                imageSize = view.Size();
                Point2f[] pointBuf;

                // 查找棋盘角点
                found = Cv2.FindChessboardCorners(view, BoardSize, out pointBuf, ChessboardFlags.AdaptiveThresh | ChessboardFlags.NormalizeImage);

                if (found)
                {
                    // 灰度化
                    Mat viewGray = new Mat();
                    Cv2.CvtColor(view, viewGray, ColorConversionCodes.BGR2GRAY);

                    // 亚像素精确化
                    Cv2.CornerSubPix(viewGray, pointBuf, new OpenCvSharp.Size(winSize, winSize), new OpenCvSharp.Size(-1, -1), new TermCriteria(CriteriaTypes.Eps | CriteriaTypes.Count, 30, 0.0001));

                    // 存储角点坐标
                    imagesPoints.Add(pointBuf);
                    p = Mat.FromArray<Point2f>(pointBuf);

                    // 在图像上绘制角点
                    Cv2.DrawChessboardCorners(view, BoardSize, pointBuf, found);
                    Mat temp = view.Clone();
                    Cv2.ImShow("Image View", view);
                }
            }

            Mat[] rvecs = new Mat[0];
            Mat[] tvecs = new Mat[0];

            // 运行相机标定
            RunCalibration(1, imageSize, out cameraMatrix, out distCoeffs, new Mat[] { p }, out rvecs, out tvecs, out double totalAvgErr);

            // 相机矩阵、畸变系数和平均误差
            sb.AppendLine(string.Format("相机矩阵:\n{0}", Cv2.Format(cameraMatrix) + "\n"));
            sb.AppendLine(string.Format("畸变系数:\n{0}", Cv2.Format(distCoeffs) + "\n"));
            sb.AppendLine(string.Format("平均误差:\n{0}", totalAvgErr + "\n"));

            // 畸变校正
            Mat map1 = new Mat();
            Mat map2 = new Mat();
            Mat newCameraMatrix = Cv2.GetOptimalNewCameraMatrix(cameraMatrix, distCoeffs, imageSize, 1, imageSize, out Rect roi);
            Cv2.InitUndistortRectifyMap(cameraMatrix, distCoeffs, new Mat(), newCameraMatrix, imageSize, MatType.CV_16SC2, map1, map2);

            // 显示校正后的图像
            Mat temp2 = Cv2.ImRead(image_path, ImreadModes.Color);
            Mat rview = new Mat();

            // 校正
            Cv2.Remap(temp2, rview, map1, map2, InterpolationFlags.Linear);

            double costTime = stopwatch.Elapsed.TotalMilliseconds;

            sb.AppendLine( $"\r\n耗时:{costTime:F2}ms");
            textBox1.Text = sb.ToString();
            pictureBox2.Image = new Bitmap(rview.ToMemoryStream());

        }

        // 运行相机标定
        private void RunCalibration(int imagesCount, OpenCvSharp.Size imageSize, out Mat cameraMatrix, out Mat distCoeffs, Mat[] imagePoints, out Mat[] rvecs, out Mat[] tvecs, out double totalAvgErr)
        {
            // 初始化相机矩阵和畸变系数
            cameraMatrix = Mat.Eye(new OpenCvSharp.Size(3, 3), MatType.CV_64F);
            distCoeffs = Mat.Zeros(new OpenCvSharp.Size(8, 1), MatType.CV_64F);

            // 计算棋盘角点的世界坐标
            Mat[] objectPoints = CalcBoardCornerPositions(BoardSize, SquareSize, imagesCount);

            // 进行相机标定
            double rms = Cv2.CalibrateCamera(objectPoints, imagePoints, imageSize, cameraMatrix, distCoeffs, out rvecs, out tvecs, CalibrationFlags.None);

            // 检查相机矩阵和畸变系数的范围
            bool ok = Cv2.CheckRange(InputArray.Create(cameraMatrix)) && Cv2.CheckRange(InputArray.Create(distCoeffs));

            // 计算重投影误差
            totalAvgErr = ComputeReprojectionErrors(objectPoints, imagePoints, rvecs, tvecs, cameraMatrix, distCoeffs);
        }

        // 计算棋盘角点的世界坐标
        private Mat[] CalcBoardCornerPositions(OpenCvSharp.Size BoardSize, float SquareSize, int imagesCount)
        {
            Mat[] corners = new Mat[imagesCount];
            // 遍历每张图片
            for (int k = 0; k < imagesCount; k++)
            {
                Point3f[] p = new Point3f[BoardSize.Height * BoardSize.Width];

                for (int i = 0; i < BoardSize.Height; i++)
                {
                    for (int j = 0; j < BoardSize.Width; j++)
                    {
                        // 计算每个格子的三维坐标并储存在一维数组 p 中
                        p[i * BoardSize.Width + j] = new Point3f(j * SquareSize, i * SquareSize, 0);
                    }
                }
                // 将三维坐标转换成 Mat 类型并存储再 corners 数组中
                corners[k] = Mat.FromArray<Point3f>(p);
            }
            return corners;
        }

        // 计算重投影误差
        private double ComputeReprojectionErrors(Mat[] objectPoints, Mat[] imagePoints, Mat[] rvecs, Mat[] tvecs, Mat cameraMatrix, Mat distCoeffs)
        {
            Mat imagePoints2 = new Mat();
            int totalPoints = 0;
            double totalErr = 0, err;

            for (int i = 0; i < objectPoints.Length; ++i)
            {
                Cv2.ProjectPoints(objectPoints[i], rvecs[i], tvecs[i], cameraMatrix, distCoeffs, imagePoints2);

                err = Cv2.Norm(imagePoints[i], imagePoints2, NormTypes.L2);

                int n = objectPoints[i].Width * objectPoints[i].Height;
                totalErr += err * err;
                totalPoints += n;
            }

            return Math.Sqrt(totalErr / totalPoints);
        }

        private void button3_Click(object sender, EventArgs e)
        {
            if (pictureBox2.Image == null)
            {
                return;
            }
            Bitmap output = new Bitmap(pictureBox2.Image);
            var sdf = new SaveFileDialog();
            sdf.Title = "保存";
            sdf.Filter = "Images (*.jpg)|*.jpg|Images (*.png)|*.png|Images (*.bmp)|*.bmp|Images (*.emf)|*.emf|Images (*.exif)|*.exif|Images (*.gif)|*.gif|Images (*.ico)|*.ico|Images (*.tiff)|*.tiff|Images (*.wmf)|*.wmf";
            if (sdf.ShowDialog() == DialogResult.OK)
            {
                switch (sdf.FilterIndex)
                {
                    case 1:
                        {
                            output.Save(sdf.FileName, ImageFormat.Jpeg);
                            break;
                        }
                    case 2:
                        {
                            output.Save(sdf.FileName, ImageFormat.Png);
                            break;
                        }
                    case 3:
                        {
                            output.Save(sdf.FileName, ImageFormat.Bmp);
                            break;
                        }
                    case 4:
                        {
                            output.Save(sdf.FileName, ImageFormat.Emf);
                            break;
                        }
                    case 5:
                        {
                            output.Save(sdf.FileName, ImageFormat.Exif);
                            break;
                        }
                    case 6:
                        {
                            output.Save(sdf.FileName, ImageFormat.Gif);
                            break;
                        }
                    case 7:
                        {
                            output.Save(sdf.FileName, ImageFormat.Icon);
                            break;
                        }
                    case 8:
                        {
                            output.Save(sdf.FileName, ImageFormat.Tiff);
                            break;
                        }
                    case 9:
                        {
                            output.Save(sdf.FileName, ImageFormat.Wmf);
                            break;
                        }
                }
                MessageBox.Show("保存成功，位置：" + sdf.FileName);
            }
        }

    }
}