using OpenCvSharp;
using OpenCvSharp.Extensions;
using PaintDotNet.Setting;
using StageController;
using System;
using System.Collections.Generic;
using System.Drawing;
using System.Linq;
using System.Text;
using System.Threading;
using System.Threading.Tasks;

namespace PaintDotNet
{
    public class AutoFocusWorkflow
    {
        public static bool IsWorking;
        public static double CurrentValue;
        public static void Stop()
        {
            IsWorking = false;
        }

        /// <summary>        
        /// 自动聚焦的线程方法
        /// </summary>
        public static string AutoFocus(AxisController stage, Func<Bitmap> CurrentImage)
        {
            if (!stage.IsOpen) return "载物台离线";
            if (IsWorking) return "正在聚焦中......";

            IsWorking = true;

            int round = 0;//翻转次数

            double pulse = FocusingParameter.getRuleFocus().StepLength;
            var focusRange = pulse * 5;

            double lastMaxMean = 0.0;
            double maxMeanValue = 0.0;

            int dir = -1; //方向

            double trip = 0.0;
            try
            {
                while (IsWorking)
                {
                    stage.WaitMoveDone();
                    stage.SetSpeedZ(Math.Max((AxisController.ControllerType.Equals("HDS") ? 200 : 80) / (round + 1), 17));


                    Bitmap m_bitmap = CurrentImage();
                    CurrentValue = getMeanValueOfBitmap(m_bitmap);
                    Console.WriteLine(string.Format("自动聚焦:{0:f3}   Z:{1:f3}", CurrentValue, stage.Z));



                    bool foundNextMaxMean = false;
                    if (CurrentValue > lastMaxMean)
                    {
                        foundNextMaxMean = true;
                        lastMaxMean = CurrentValue;
                    }

                    if (CurrentValue > maxMeanValue)
                    {
                        maxMeanValue = CurrentValue;
                    }

                    if (!foundNextMaxMean)
                    {
                        dir = -dir;

                        if (pulse == 1)  //结束
                        {
                            if (dir > 0)
                                stage.Up(1.7);
                            else
                                stage.Up(-0.6);
                            // MessageBox.Show("Done.");
                            return "";
                        }
                        Console.WriteLine("Reverse:" + (dir > 0 ? "+" : "-"));
                        round++;
                        lastMaxMean = 0;
                        pulse = (int)(Math.Max(1, pulse * 0.3));

                    }
                    trip += pulse * dir;
                    if (Math.Abs(trip) > focusRange)
                    {
                        return "超出聚焦行程,聚焦结束";
                    }
                    if (AxisController.ControllerType.Equals("HDS") && pulse <= 2 && pulse >= -2)  //结束
                        return "聚焦到最小距离,聚焦结束";
                    stage.Up(pulse * dir);
                }
                return "聚焦中断";
            }
            catch (Exception ex)
            {
                return "聚焦中断";
            }
            finally
            {
                stage.WaitMoveDone();

                IsWorking = false;

                stage.FreeZ();
            }
        }


        public static void AutoFocusFast(AxisController stage, Func<Bitmap> CurrentImage)
        {
            if (IsWorking)
                return;

            IsWorking = true;

            int round = 0;//翻转次数
            double pulse = FocusingParameter.getRuleFocus().StepLength / 4;//最小步长1.25um
            double lastValue = 0.0;
            int dir = -1; //方向
            double trip = 0.0;
            //stage.SetSpeedZ(40);
            try
            {
                while (IsWorking)
                {
                    //Console.Write("Before focus:");
                    stage.WaitMoveDone();
                    //Console.Write("自动聚焦:");
                    Thread.Sleep(40);
                    try
                    {
                        Bitmap m_bitmap = CurrentImage();
                        CurrentValue = getMeanValueOfBitmap(m_bitmap);
                        //Console.WriteLine(string.Format("{0:f3}  Z:{1:f3}", m_CurrentValue, stage.Z));
                    }
                    catch (Exception ex)
                    {
                        return;
                    }

                    if (round > 1) return;
                    else if (lastValue == 0) ;
                    else if (CurrentValue > lastValue)
                    {
                        round = 1;
                    }
                    else
                    {
                        //Console.WriteLine("Reverse:" + (dir > 0 ? "+" : "-"));
                        round++;
                        dir = -dir;
                    }
                    lastValue = CurrentValue;

                    trip += pulse * dir;
                    if (Math.Abs(trip) > pulse * 5)
                    {
                        Console.WriteLine("Trip of Z out of autofocus range.");
                        return;
                    }
                    stage.Up(pulse * dir);
                }
            }
            catch
            {
            }
            finally
            {
                // Console.Write("End focus:");
                stage.WaitMoveDone();

                //m_CurrentValue = getMeanValueOfBitmap(CurrentImage());
                //Console.WriteLine(string.Format("聚焦完成:{0:f3}   Z:{1:f3}", m_CurrentValue, stage.Z));
                IsWorking = false;
                // stage.LockZ();
            }
        }
        /// <summary>
        /// 获取输入图片的清晰度
        /// </summary>
        /// <param name="bitmap"></param>
        /// <returns></returns>
        private static double getMeanValueOfBitmap(Bitmap bitmap)
        {
            Mat converted = PaintDotNet.Camera.Tools.ToMat(bitmap);
            Mat imageGrey = new Mat();
            Mat imageSobel = new Mat();
            try
            {
                if (converted.Channels() == 3)
                    OpenCvSharp.Cv2.CvtColor(converted, imageGrey, OpenCvSharp.ColorConversionCodes.RGB2GRAY);
                else if (converted.Channels() == 1)
                    imageGrey = converted;
            }
            catch (Exception)
            {
                imageGrey = converted;
            }

            OpenCvSharp.Mat meanValueImage = new OpenCvSharp.Mat();
            OpenCvSharp.Mat meanStdValueImage = new OpenCvSharp.Mat();

            //求灰度图像的标准差  值越大越好 
            OpenCvSharp.Cv2.MeanStdDev(imageGrey, meanValueImage, meanStdValueImage);
            return meanStdValueImage.At<double>(0, 0);
        }
    }
}