using System;
using System.Collections.Generic;
using System.Drawing;
using System.Linq;
using System.Threading;
using System.Windows.Forms;
using AForge.Video;
using AForge.Video.DirectShow;
using log4net;
using OpenCvSharp;
using OpenCvSharp.CPlusPlus;
using OpenCvSharp.Extensions;
using Ysoft.Rqamfp.Rob;
using YSoft.Rqa.Points;
using Point = OpenCvSharp.CPlusPlus.Point;
using Screen = Ysoft.Rqamfp.Rob.MFD.Screen;
using Size = OpenCvSharp.CPlusPlus.Size;

namespace YSoft.Rqamfp.Camera
{
    public partial class CameraCalibration : Form
    {
        // gray snapped image
        private Mat _image;
        // color snapped image
        private Mat _imgRgb;
        // mat containing user points and theirs indicators
        private Mat _texts;
        // mat for method updatepic, which contains _image and _texts according to edge method
        private Mat _imgTresh;
        // Camera index
        private int _index;
        // transformed image
        private Mat _transImage;
        private VideoCaptureDevice _video;
        static CancellationTokenSource _cts = new CancellationTokenSource();
        // array of corners found by opencv method
        private Point2f[] _corners;
        private int _actualFrame = 1;
        // user selected points
        private readonly List<Point2f> _userPoints = new List<Point2f>();
        // points which are nearest to the user points, these are used as transformation points
        private readonly List<Point2f> _foundCorners = new List<Point2f>();
        private static readonly ILog Logger = LogManager.GetLogger(typeof(CameraCalibration));
        private Window _pictureWindow;
        public event EventHandler IdentifyEvent;

        public CameraCalibration() {
            InitializeComponent();
            SetDefaultValues();
        }

        private void OnLoad(object sender, EventArgs e) {
            GetAllCameras();
            selectedCamera.SelectedIndex = Camera.CameraIndex;
            _pictureWindow = new Window("picture");
            _pictureWindow.OnMouseCallback += (OnMarking);
            Camera.Calibrate = false;
        }

        private void SetDefaultValues() {
            IP1.Text = Properties.Settings.Default.ip1;
            IP2.Text = Properties.Settings.Default.ip2;
            IP3.Text = Properties.Settings.Default.ip3;
            IP4.Text = Properties.Settings.Default.ip4;
            IP5.Text = Properties.Settings.Default.ip5;
        }

        private void SaveDefaultValues() {
            Properties.Settings.Default.ip1 = IP1.Text;
            Properties.Settings.Default.ip2 = IP2.Text;
            Properties.Settings.Default.ip3 = IP3.Text;
            Properties.Settings.Default.ip4 = IP4.Text;
            Properties.Settings.Default.ip5 = IP5.Text;
            Properties.Settings.Default.Save();
        }

        private void GetAllCameras() {
            var videoDevices = new FilterInfoCollection(FilterCategory.VideoInputDevice);
            for (int i = 0; i < videoDevices.Count; i++) {
                selectedCamera.Items.Add(i + ". " + videoDevices[i].Name);
            }
        }

        private void ShowVideo() {
            Logger.Info("Initializing video stream with camera of index: " + _index);
            VideoCapture capt = new VideoCapture(CaptureDevice.Any, _index);
            Mat frame = new Mat();
            _cts = new CancellationTokenSource();
            while (capt.Grab() && !_cts.IsCancellationRequested && capt.IsOpened()) {
                try {
                    capt.Retrieve(frame, 0);
                    Cv2.ImShow("video stream", frame);
                    Cv2.WaitKey(25);
                } catch (Exception ex) {
                    Logger.Debug("Something went wrong during video stream, however this exception is not relevant", ex);
                }
            }
            capt.Release();
            Cv2.DestroyWindow("video stream");
        }

        private void UpdateEvent(object sender, EventArgs e) {
            switch (edgeMethod.SelectedIndex) {
                case 0:
                    cannyset.Visible = true;
                    cannyset.Enabled = true;
                    threshset.Visible = false;
                    threshset.Enabled = false;
                    break;
                case 1:
                    cannyset.Visible = false;
                    cannyset.Enabled = false;
                    threshset.Visible = true;
                    threshset.Enabled = true;
                    break;
                default:
                    cannyset.Visible = false;
                    cannyset.Enabled = false;
                    threshset.Visible = false;
                    threshset.Enabled = false;
                    break;
            }
            UpdatePic();
        }

        // updates displayed picture with newest content
        private void UpdatePic() {
            _pictureWindow.OnMouseCallback -= (OnMarking);
            if (_image == null) {
                Logger.Debug("Updating a window without Image initialized");
                return;
            }
            switch (edgeMethod.SelectedIndex) {
                case -1:
                    _imgTresh = _image.Resize(new Size(1280, 720));
                    break;
                case 0:
                    Cv2.Canny(_image, _imgTresh, (double) LowT.Value, (double) HighT.Value, 3, true);
                    break;
                case 1:
                    int numb = (int) number.Value;
                    Cv2.AdaptiveThreshold(_image,
                        _imgTresh,
                        numb,
                        AdaptiveThresholdType.GaussianC,
                        ThresholdType.Binary,
                        (int) numbBlock.Value,
                        (double) numb2.Value);
                    break;
                case 2:
                    Mat diag1Mat = new Mat();
                    Mat diag2Mat = new Mat();
                    Mat subTmp2 = new Mat();
                    Mat subTmp1 = new Mat();
                    Mat sobelV = new Mat(new Size(_imgTresh.Width, _imgTresh.Height), _imgTresh.Type());
                    Mat sobelH = new Mat(new Size(_imgTresh.Width, _imgTresh.Height), _imgTresh.Type());
                    Cv2.Sobel(_image, sobelV, -1, 0, 1);
                    Cv2.Sobel(_image, sobelH, -1, 1, 0);
                    Cv2.AddWeighted(sobelV, 0.5, sobelH, 0.5, 0, subTmp1);
                    // applying also both diagonal sobels
                    float[] diag1 = {0, 1, 2, -1, 0, 1, -2, -1, 0};
                    float[] diag2 = {-2, -1, 0, -1, 0, 1, 0, 1, 2};
                    Cv2.Filter2D(_image, diag1Mat, -1, new Mat(3, 3, MatType.CV_32FC1, diag1));
                    Cv2.Filter2D(_image, diag2Mat, -1, new Mat(3, 3, MatType.CV_32FC1, diag2));
                    Cv2.AddWeighted(diag1Mat, 0.5, diag2Mat, 0.5, 0, subTmp2);
                    Cv2.AddWeighted(subTmp1, 0.5, subTmp2, 0.5, 0, _imgTresh);
                    break;
                case 3:
                    Cv2.Laplacian(_image, _imgTresh, -1, 3);
                    break;
                case 4:
                    _imgTresh = _image.Resize(new Size(1280, 720));
                    break;
                default:
                    Cv2.Canny(_image, _imgTresh, (double) LowT.Value, (double) HighT.Value);
                    break;
            }
            Mat tmpTresh = new Mat();
            Mat thresResMat = _imgTresh.Resize(new Size(1280, 720));
            Mat textsResMat = _texts.Resize(new Size(1280, 720));
            if (_imgTresh.Channels() == 1)
                Cv2.CvtColor(thresResMat, tmpTresh, ColorConversion.GrayToRgb);
            _pictureWindow.ShowImage((_imgTresh.Channels() == 1 ? tmpTresh : thresResMat) + textsResMat);
            _pictureWindow.OnMouseCallback += (OnMarking);
        }

        private void OnMarking(MouseEvent eventType, int x, int y, MouseEvent flags) {
            if (eventType == MouseEvent.LButtonDoubleClick) {
                if (_image == null || _texts == null)
                    return;
                int xT = (int) (((float) x/1280)*1920);
                int yT = (int) (((float) y/720)*1080);
                var inc = y > _image.Height/2 ? 40 : -20;
                Point org = new Point(xT + inc/2, yT + inc);
                Cv2.Circle(_texts, xT, yT, 3, Scalar.Aqua, 3);
                Cv2.Circle(_texts, xT, yT, 20, Scalar.Aqua, 1, LineType.AntiAlias);
                Cv2.PutText(_texts, xT + "," + yT, org, FontFace.HersheySimplex, 0.5, Scalar.Aqua, 1, LineType.AntiAlias);
                if (_userPoints.Count < 4) {
                    _userPoints.Add(new Point2f(xT, yT));
                }
                else if (MessageBox.Show("Points are already assigned, do you want to start again?",
                    "Already assigned points",
                    MessageBoxButtons.YesNo) == DialogResult.Yes) {
                    _userPoints.Clear();
                    _foundCorners.Clear();
                    SearchCorners();
                }
                UpdatePic();
            }
        }

        private void ClearTextsOnClick(object sender, EventArgs e) {
            ResetMatsVectors();
            _foundCorners.Clear();
            UpdatePic();
        }

        private void FindCornersOnClick(object sender, EventArgs e) => SearchCorners();

        private void SearchCorners() {
            if (_image == null || _imgTresh == null) {
                MessageBox.Show("First snap an image");
                return;
            }
            _texts = new Mat(new Size(_image.Width, _image.Height), MatType.CV_8UC3);
            _corners = new Point2f[0];
            _corners = Cv2.GoodFeaturesToTrack(_imgTresh, 30000, 0.1, 5, new Mat(), 5, false, 0.04);
            var color = Scalar.Red;
            foreach (Point2f point in _corners) {
                Cv2.Circle(_texts, (int) point.X, (int) point.Y, 5, color, 2);
            }
            UpdatePic();
        }

        private void BestMatchesOnClick(object sender, EventArgs e) {
            if (_userPoints.Count == 0) {
                MessageBox.Show("First you need to mark calibration points");
                return;
            }
            if (_foundCorners.Count > 0) {
                MessageBox.Show("You already have attached points, use Clear points button to attach new points");
                return;
            }
            if (_corners == null && !manualCorners.Checked) {
                MessageBox.Show("First you need to detect corners or check Manual transformation");
                return;
            }
            foreach (var corn in _userPoints) {
                if (manualCorners.Checked) {
                    _foundCorners.Add(corn);
                    Cv2.Circle(_texts, Convert.ToInt32(corn.X), Convert.ToInt32(corn.Y), 5, Scalar.Green, 2);
                    continue;
                }
                var corners = _corners.Where(x => Math.Abs(x.X - corn.X) < 20 && Math.Abs(x.Y - corn.Y) < 20);
                var corner = corners.FirstOrDefault(x => x.DistanceTo(corn) == corners.Min(y => y.DistanceTo(corn)));
                _foundCorners.Add(corner);
                Cv2.Circle(_texts, Convert.ToInt32(corner.X), Convert.ToInt32(corner.Y), 5, Scalar.Green, 2);
                Console.WriteLine("Nearest point: " + corner.X + "," + corner.Y + " for calibration point:" +
                                  corn.X + "," + corn.Y);
            }
            UpdatePic();
        }

        // transforming marked image into 1280x720
        private Mat TransformImage() {
            if (_foundCorners.Count != 4) {
                return null;
            }
            Point2f[] srcPoints = new Point2f[4];
            Point2f[] dstPoints = new Point2f[4];
            Mat warpDest = new Mat(new Size(1280, 720), MatType.CV_8UC3);
            Mat transformed = new Mat(new Size(1280, 720), MatType.CV_8UC3);
            for (int i = 0; i < 4; i++) {
                srcPoints[i] = _foundCorners[i];
            }
            dstPoints[0] = new Point2f(0, 720);
            dstPoints[1] = new Point2f(0, 0);
            dstPoints[2] = new Point2f(1280, 0);
            dstPoints[3] = new Point2f(1280, 720);
            // transformation matrix
            var translate = Cv2.GetPerspectiveTransform(srcPoints, dstPoints);
            Cv2.WarpPerspective(_imgRgb, transformed, translate, warpDest.Size(), Interpolation.Cubic);
            Mat gauss = new Mat();
            Cv2.GaussianBlur(transformed, gauss, new Size(0, 0), 5);
            Cv2.AddWeighted(transformed, 1.5, gauss, -0.5, 0, transformed);
            return transformed;
        }

        private bool CalibrationAcceptation() {
            DialogResult result = MessageBox.Show("Do you want to calibrate Image processing server for current IP?",
                "Calibration acceptation",
                MessageBoxButtons.YesNo);
            return result != DialogResult.No;
        }

        private void CalibrateIpOnClick(object sender, EventArgs e) {
            if (new[] {IP1.Text, IP2.Text, IP3.Text, IP4.Text, IP5.Text}.Contains(string.Empty)) {
                MessageBox.Show("First fill IP Address of Image processing server");
                return;
            }
            if (_foundCorners == null) {
                MessageBox.Show("Calibration points for the image are not known, reenter points and find matches for them");
                return;
            }
            if (_transImage == null) {
                MessageBox.Show("First, you need to transform the image");
                return;
            }
            if (!CalibrationAcceptation())
                return;
            SaveDefaultValues();
            Camera.CameraCalibration = _foundCorners;
            Camera.ImageProcessingServer = IP1.Text + "." + IP2.Text + "." + IP3.Text + "." + IP4.Text + ":" + IP5.Text;
            Cv2.DestroyAllWindows();
            DialogResult = DialogResult.OK;
            Camera.Calibrate = true;
            Close();
        }

        // snapping is realised through video stream, which is not ideal, but current camera does not supports static snapping
        private void SnapOnClick(object sender, EventArgs e) {
            _cts.Cancel();
            Cv2.WaitKey(200);
            var videoDevices = new FilterInfoCollection(FilterCategory.VideoInputDevice);
            _video = new VideoCaptureDevice(videoDevices[_index].MonikerString);
            var camera = _video.VideoCapabilities.Select((Value, Index) => new {Value, Index})
                .SingleOrDefault(o => o.Value.FrameSize.Height == 1080 && o.Value.FrameSize.Width == 1920);
            if (camera == null) {
                MessageBox.Show("Selected camera does not support full HD snapping, select another camera");
                return;
            }
            _video.VideoResolution = _video.VideoCapabilities[camera.Index];
            _video.NewFrame += VideoOnNewFrame;
            _video.Start();
            _video.WaitForStop();
            _video.NewFrame -= VideoOnNewFrame;
            _video.Stop();
            _video = null;
            Camera.LastImage = _imgRgb;
            UpdatePic();
        }

        private void VideoOnNewFrame(object sender, NewFrameEventArgs eventArgs) {
            if (_actualFrame < 5) {
                _cts.Cancel();
                using (Bitmap bitmap = (Bitmap)eventArgs.Frame.Clone())
                {
                    Mat mat = bitmap.ToMat();
                    if (_actualFrame == 1) {
                        _image = new Mat();
                        Cv2.CvtColor(mat, _image, ColorConversion.RgbToGray);
                        _imgRgb = mat;
                    }
                    else {
                        Cv2.AddWeighted(_image, 0.5, mat.CvtColor(ColorConversion.RgbToGray), 0.5, 0, _image);
                        Cv2.AddWeighted(_imgRgb, 0.5, mat, 0.5, 0, _imgRgb);
                    }
                    _actualFrame++;
                }
            }
            else {
                ResetMatsVectors();
                _video.SignalToStop();
                _actualFrame = 1;
            }
        }

        private void ResetMatsVectors() {
            _texts = new Mat(new Size(_image.Width, _image.Height), MatType.CV_8UC3);
            _imgTresh = new Mat(new Size(_image.Width, _image.Height), MatType.CV_8UC1);
            _userPoints.Clear();
        }

        private void RecalibrateCameraPosOnClick(object sender, EventArgs e) {
            Cv2.DestroyAllWindows();
            ThreadPool.QueueUserWorkItem(o => ShowVideo());
        }

        private void CameraSelectionOnChange(object sender, EventArgs e) {
            Camera.CameraIndex = selectedCamera.SelectedIndex;
            _index = selectedCamera.SelectedIndex;
            Logger.Info("Camera index was set to: " + Camera.CameraIndex);
            _cts.Cancel();
        }

        private void StopVideoOnClick(object sender, EventArgs e) {
            _cts.Cancel();
        }

        private void CameraSettingsOnClick(object sender, EventArgs e) {
            CameraSettingsControl csc = new CameraSettingsControl();
            csc.ShowDialog();
        }

        private void ManualCornersOnChange(object sender, EventArgs e) {
            findCorners.Enabled = !manualCorners.Checked;
        }

        private void DbFillerStartOnClick(object sender, EventArgs e) {
            if (_transImage == null) {
                MessageBox.Show("Calibration points for the image are not known, reenter points");
                return;
            }
            Cv2.DestroyAllWindows();
            DbFiller dbFiller = new DbFiller {Image = _transImage};
            dbFiller.Show();
        }

        private void CloseOnClick(object sender, EventArgs e) {
            Cv2.DestroyAllWindows();
            Close();
        }

        private void ReferenceOnClick(object sender, EventArgs e) {
            if (_transImage == null) {
                MessageBox.Show("Calibration points for the image are not known, reenter points");
                return;
            }
            Cv2.DestroyAllWindows();
            SaveFileDialog dialog = new SaveFileDialog {
                FileName = "Title of screen",
                Filter = "PNG format (*.png)|*.png"
            };
            try {
                dialog.ShowDialog();
                Cv2.ImWrite(dialog.FileName, _transImage);
            } catch (Exception ex) {
                Logger.Debug("Dialog window was closed", ex);
            }
        }

        private void SetCalibrationPointsOnClick(object sender, EventArgs e) {
            if (_imgRgb == null || _foundCorners.Count != 4)
                return;
            _transImage = new Mat(new Size(1280, 720), MatType.CV_8UC3);
            _transImage = TransformImage();
            previewImage.Image = _transImage.ToBitmap();
        }

        private void ShowTransformedOnClick(object sender, EventArgs e) {
            _transImage = new Mat(new Size(1280, 720), MatType.CV_8UC3);
            _transImage = TransformImage();
            if (_foundCorners == null || _foundCorners.Count == 0) {
                MessageBox.Show("Transformation points are not set");
                return;
            }
            if (_transImage != null)
                Cv2.ImShow("transformed", _transImage);
        }

        private void ReshowImageOnClick(object sender, EventArgs e) {
            UpdatePic();
        }

        private void OnClosing(object sender, FormClosingEventArgs e) {
            _pictureWindow.Close();
            _pictureWindow.Dispose();
        }

        private void SaveAndCloseOnClick(object sender, EventArgs e) {
            Camera.ImageProcessingServer = IP1.Text + "." + IP2.Text + "." + IP3.Text + "." + IP4.Text + ":" + IP5.Text;
            SaveDefaultValues();
            Close();
        }

        private async void MoveRobotOnClick(object sender, EventArgs e) {
            ((Button) sender).Enabled = false;
            if (!Screen.Instance.IsCalibrated || Screen.Instance.Transition.ScreenSize == null) {
                MessageBox.Show("Robot is not calibrated, moving is canceled");
                return;
            }
            try {
                if (Robot.Instance._state != Robot.RobotState.Ready)
                    await Robot.Instance.WakeUp();
                await Robot.Instance.MoveToPos(new Point4(95, 8, 14, 90));
            } catch (Exception ex) {
                Logger.Error("Error during moving the robot", ex);
            }
            finally {
                ((Button) sender).Enabled = true;
            }
        }

        private void UseCalibratedPointsOnClick(object sender, EventArgs e) {
            if (Camera.CameraCalibration == null ||Camera.CameraCalibration.Count != 4) {
                MessageBox.Show("There are no calibration points");
                return;
            }

            if (_image == null) {
                MessageBox.Show("First snap an image");
                return;
            }
            _foundCorners.Clear();
            Camera.CameraCalibration.ForEach(x => _foundCorners.Add(x));
            _texts = new Mat(new Size(_image.Width, _image.Height), MatType.CV_8UC3);
            Camera.CameraCalibration.ForEach(x => Cv2.Circle(_texts, (int) x.X, (int) x.Y, 3, Scalar.Aqua, 3));
            Camera.CameraCalibration.ForEach(x => Cv2.Circle(_texts, (int) x.X, (int) x.Y, 20, Scalar.Aqua, 1, LineType.AntiAlias));
            if (_imgRgb == null || _foundCorners.Count != 4)
                return;
            _transImage = new Mat(new Size(1280, 720), MatType.CV_8UC3);
            _transImage = TransformImage();
            previewImage.Image = _transImage.ToBitmap();
            UpdatePic();
        }

        private void IdentifyOnClick(object sender, EventArgs e)
        {
            if (Camera.LastImage == null) {
                MessageBox.Show("No image set");
            }
            IdentifyEvent.Invoke(sender,e);
        }
    }
}
