using HalconDotNet;
using LB_VisionControl;
using Newtonsoft.Json;
using Newtonsoft.Json.Serialization;
using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Diagnostics;
using System.Linq;
using System.Runtime.Intrinsics.X86;
using System.Text;
using System.Threading.Tasks;

namespace LB_VisionProcesses.Alogrithms.Halcon
{
    public class BaseCalib
    {
        #region 标定板
        // Procedures 
        // Chapter: Calibration / Camera Parameters
        // Short Description: Generate a camera parameter tuple for an area scan camera with distortions modeled by the division model. 
        /// <summary>
        /// 面阵相机内参生成
        /// </summary>
        /// <param name="hv_Focus"></param>
        /// <param name="hv_Kappa"></param>
        /// <param name="hv_Sx"></param>
        /// <param name="hv_Sy"></param>
        /// <param name="hv_Cx"></param>
        /// <param name="hv_Cy"></param>
        /// <param name="hv_ImageWidth"></param>
        /// <param name="hv_ImageHeight"></param>
        /// <param name="hv_CameraParam"></param>
        public static void gen_cam_par_area_scan_division(HTuple hv_Focus, HTuple hv_Kappa, HTuple hv_Sx,
            HTuple hv_Sy, HTuple hv_Cx, HTuple hv_Cy, HTuple hv_ImageWidth, HTuple hv_ImageHeight,
            out HTuple hv_CameraParam)
        {
            // Local iconic variables 
            // Initialize local and output iconic variables 
            hv_CameraParam = new HTuple();
            //Generate a camera parameter tuple for an area scan camera
            //with distortions modeled by the division model.
            //
            hv_CameraParam.Dispose();
            using (HDevDisposeHelper dh = new HDevDisposeHelper())
            {
                hv_CameraParam = new HTuple();
                hv_CameraParam[0] = "area_scan_division";
                hv_CameraParam = hv_CameraParam.TupleConcat(hv_Focus, hv_Kappa, hv_Sx, hv_Sy, hv_Cx, hv_Cy, hv_ImageWidth, hv_ImageHeight);
            }
            return;
        }

        /// <summary>
        /// 生成棋盘格标定板文件
        /// </summary>
        /// <param name="dirPath"></param>
        /// <param name="size"></param>
        /// <param name="count"></param>
        public static void CreateCaltab(string dirPath = @"C://", int XNum = 7, int YNum = 7
            , double MarkDist = 1.25, double DiameterRatio = 0.5)
        {
            try
            {
                //XNum：每行黑色标志圆点的数量。
                //YNum：每行黑色标志圆点的数量。
                //MarkDist：两个就近黑色圆点中心之间的距离。
                //DiameterRstio：黑色圆点半径与圆点中心距离的比值。
                //CalPlateDescr：标定板描述文件的文件路径（.descr文件为标定板描述文件）。
                //CalPlatePSFile：标定板图像文件的文件路径（.ps文件为标定板图形文件,可利用PS类软件打开）。

                string CalPlateDescr = dirPath + $"Caltab_{XNum}*{YNum}_{DiameterRatio}.descr";
                string CalPlatePSFile = dirPath + $"Caltab_{XNum}*{YNum}_{DiameterRatio}.ps";
                HOperatorSet.GenCaltab(XNum, YNum, MarkDist / 1000.0, DiameterRatio, CalPlateDescr, CalPlatePSFile);
            }
            catch { }
        }

        public static void CaltabCalib(HTuple hv_CameraParam, List<HObject> lst_Images
            , out HTuple CameraInner, out HTuple CameraPose, out HTuple CameraInner_Adaptive
            , out HObject Map, out HObject CircleXld
            , string CalPlateDescr = "C:/Caltab.descr"
            , string CameraInnerPath = "C:/CameraInner_Adaptive.tup", string CameraPosePath = "C:/CameraPose.tup")
        {
            CameraInner = null; CameraPose = null; CameraInner_Adaptive = null; Map = null; CircleXld = null;
            try
            {
                //1.获取相机参数
                //using (HDevDisposeHelper dh = new HDevDisposeHelper())
                //{
                //    hv_CameraParam.Dispose();
                //    gen_cam_par_area_scan_division(0.008, 0, 5.2e-06, 5.2e-06, 3072 / 2, 2048 / 2, 3072,
                //        2048, out hv_CameraParam);
                //}

                //2.创建标定对象
                HOperatorSet.CreateCalibData("calibration_object", 1, 1, out HTuple hv_CalibDataID);

                //3.在标定对象中设置相机参数
                HOperatorSet.SetCalibDataCamParam(hv_CalibDataID, 0, new HTuple(), hv_CameraParam);

                //4.在标定对象中设置标定板描述文件
                HOperatorSet.SetCalibDataCalibObject(hv_CalibDataID, 0, CalPlateDescr);

                //5.加载标定图像，进行标定

                //- 标定板不要过曝，不要出现255的灰度值
                //- 光照要均匀
                //- 标定板特征点的对比度要高，黑白区域灰度值差100以上
                //- 标定板在图像中至少占1 / 4面积
                //- 特征点应该对焦清晰
                //- 所有特征点应该全部落在图像范围内
                //- 标定图像至少10幅
                //- 标定板应该覆盖整个视野的各个角落
                //- 标定板角度变化要明显
                //- 图像大小要一致

                int Index = 0;
                if (lst_Images == null || lst_Images.Count == 0)
                    throw new Exception();

                // 标定图片必须为黑白
                try
                {
                    for (int i = 0; i <= lst_Images.Count; i++)
                    {
                        var ho_Image = lst_Images[i];
                        HOperatorSet.CountChannels(ho_Image, out HTuple hv_Channels);

                        if (hv_Channels.TupleInt() != 1)
                            HOperatorSet.Rgb1ToGray(ho_Image, out ho_Image);
                        lst_Images[i] = ho_Image;
                    }
                }
                catch { }

                foreach (var ho_Image in lst_Images)
                {
                    //标定图像
                    HOperatorSet.FindCalibObject(ho_Image, hv_CalibDataID, 0, 0, Index, new HTuple(),
                        new HTuple());
                    //获取轮廓
                    HOperatorSet.GetCalibDataObservContours(out HObject ho_Contours, hv_CalibDataID, "marks", 0, 0, Index);
                    //获取圆心点和位姿
                    HOperatorSet.GetCalibDataObservPoints(hv_CalibDataID, 0, 0, Index
                        , out HTuple hv_Row, out HTuple hv_Column, out HTuple _, out HTuple _);

                    HOperatorSet.GenCrossContourXld(out CircleXld, hv_Row, hv_Column, 10, 0.785398);
                }

                //6.标定
                HOperatorSet.CalibrateCameras(hv_CalibDataID, out HTuple hv_Error);
                //7.获取相机内参
                HOperatorSet.GetCalibData(hv_CalibDataID, "camera", 0, "params", out CameraInner);
                //8.获取相机外参
                HOperatorSet.GetCalibData(hv_CalibDataID, "calib_obj_pose", (new HTuple(0)).TupleConcat(
                    0), "pose", out HTuple hv_Pose);
                HOperatorSet.SetOriginPose(hv_Pose, 0, 0, 0.002, out CameraPose);

                //9.获取畸变参数
                //描述：根据指定的径向畸变系数，求取理想无畸变的相机内参。
                //参数：
                //Mode：畸变模式
                //CamParamIn：畸变的相机内部参数
                //DistortionCoeffs ：畸变系数值
                //CamParamOut：已校正的相机内参
                HOperatorSet.ChangeRadialDistortionCamPar("adaptive", CameraInner, 0, out CameraInner_Adaptive);

                //10.保存相机参数
                //相机内参
                HOperatorSet.WriteTuple(CameraInner_Adaptive, CameraInnerPath);
                //相机外参
                HOperatorSet.WriteTuple(CameraPose, CameraPosePath);

                //11.生成畸变矫正Map
                //描述：根据指定图像和指定相加参数来矫正输入图像的畸变。change_radial_distortion_image根据摄像机内部参数CamParamIn和CamParamOut对输入图像图像的径向畸变进行改变。使用CamParamOut将位于区域区域内的输出图像的每个像素转换为图像平面，然后使用CamParamIn将其投影为图像的亚像素。通过双线性插值确定得到的灰度值。如果该亚像素在图像之外，则将图像重建中的对应像素设置为“黑色”并从图像域中消除。
                //参数：
                //Image：输入图像
                //Region ：矫正图像的区域
                //ImageRectified ：矫正图像
                //CamParamIn：输入相机参数
                //CamParamOut ：输出相机参数
                HOperatorSet.GenRadialDistortionMap(out Map, CameraInner, CameraInner_Adaptive, "bilinear");
            }
            catch { CameraInner = null; CameraPose = null; CameraInner_Adaptive = null; Map = null; CircleXld = null; }
        }

        public static void ApplyCaltabCalib(HObject Image, HObject Map, out HObject MappedImage)
        {
            try
            {
                HOperatorSet.MapImage(Image, Map, out MappedImage);
            }
            catch { MappedImage = null; }
        }

        public static void ApplyCaltabCalibByPoint(HPoint imagePoint, out HPoint worldPoint, HTuple hv_CaltabCalibParams, HTuple hv_PoseCalibParams)
        {
            worldPoint = new HPoint(imagePoint);
            try
            {
                HOperatorSet.ImagePointsToWorldPlane(hv_CaltabCalibParams, hv_PoseCalibParams
                    , imagePoint.Row, imagePoint.Column, "mm", out HTuple Row, out HTuple Column);

                worldPoint.Row = Row.D;
                worldPoint.Column = Column.D;
            }
            catch { }
        }
        #endregion

        #region 棋盘格
        /// <summary>
        /// 生成棋盘格标定板文件
        /// </summary>
        /// <param name="dirPath"></param>
        /// <param name="size"></param>
        /// <param name="count"></param>
        public static void CreateCheckerBoard(string dirPath = "C:\\", int size = 17, int count = 17)
        {
            try
            {
                string saveFullPath = dirPath + $"Grid_{size}*{size}_{count}.ps";
                HOperatorSet.CreateRectificationGrid(size / 100.0, count, saveFullPath);
            }
            catch { }
        }

        public static void CheckerBoardCalib(HObject Image, out HObject Map
            , int MinContrast = 25, int Radius = 10, int SigmaSaddlePoints = 2, int Threshold = 3
            , double SigmaConnectGridPoints = 0.9, int MaxDist = 5, int GridSpacing = 45)
        {
            try
            {
                HOperatorSet.Rgb1ToGray(Image, out Image);
                //****************************标定矫正过程**************************************
                //查找结构化网格
                //MinContrast 查找网格对比度
                //Radius 查找网格半径
                HOperatorSet.FindRectificationGrid(Image, out HObject ho_GridRegion, MinContrast, Radius);
                HOperatorSet.ReduceDomain(Image, ho_GridRegion, out HObject Image_Reduced);

                //提取（鞍）点，输出Row, Col
                //'facet'：滤波器
                //Sigma：平滑系数
                //Threshold:分割阈值

                HOperatorSet.SaddlePointsSubPix(Image_Reduced, "facet", SigmaSaddlePoints, Threshold, out HTuple hv_Row, out HTuple hv_Col);
                HOperatorSet.GenCrossContourXld(out HObject ho_SaddlePoints, hv_Row, hv_Col, 20, 1);

                HOperatorSet.ConnectGridPoints(Image_Reduced, out HObject ho_ConnectingLines, hv_Row,
                    hv_Col, SigmaConnectGridPoints, MaxDist);
                //生成变换图

                HOperatorSet.GenGridRectificationMap(Image_Reduced, ho_ConnectingLines, out Map,
                    out HObject ho_Meshes, GridSpacing, 0, hv_Row, hv_Col, "bilinear");
            }
            catch { Map = null; }
        }

        public static void ApplyCheckerBoardCalibMap(HObject Image, HObject Map, out HObject MappedImage)
        {
            try
            {
                HOperatorSet.MapImage(Image, Map, out MappedImage);
            }
            catch { MappedImage = null; }
        }
        #endregion
    }

}