HawkeyeVision/Hawkeye.VisionBuilder.UI.Sources.Hawkeye/HawkeyeCameraImageSource.cs

using Inspectron.HawkEye.Protocol;
using OpenCvSharp;
using Serilog;
using System.Net;
using System.Runtime.InteropServices;
using Newtonsoft.Json;
using VisionBuilder.UI.Common;
using VisionBuilder.UI.Common.Processing;
using CameraSettings = Inspectron.HawkEye.Protocol.CameraSettings;

namespace Hawkeye.VisionBuilder.UI.Sources.Hawkeye
{
    public class HawkeyeCameraImageSource : IImageSource, IVisionBuilderModule
    {
        private readonly HawkeyeSettings _settings;
        private ImageClientTCP _client;
        private CameraSettings _cameraSettings;


        public HawkeyeCameraImageSource(HawkeyeSettings settings)
        {
            _settings = settings;
        }

        public CameraSettings Settings
        {
            get => _cameraSettings;
            set => _cameraSettings = value;
        }

        public void Open()
        {

            _client = new ImageClientTCP(new IPEndPoint(IPAddress.Parse("192.168.3.15"), 27001),
                IPAddress.Parse(_settings.Adapter));
            _client.ImageReceived += _client_ImageReceived;
            _client.SettingsReceived += _client_SettingsReceived;
            _client.CalibrationReceived += _client_CalibrationReceived;
            _client.SupportsCalibration = _settings.IsColor;
            _client.Connect();
            Log.Information("Connected to Hawkeye camera at {Adapter}", _settings.Adapter, 27001);

        }

        private void _client_CalibrationReceived(List<Dictionary<double, double>> obj)
        {
            Log.Information("Calibtating...");
            _bayerFilter = new OpenCVBayerProcessor();
            _bayerFilter.SetCalibration(obj);
        }

        private void _client_SettingsReceived(CameraSettings obj)
        {
            ApplySettings(obj);
        }
        public void ApplySettings(CameraSettings obj)
        {
            _cameraSettings= obj;
            if (File.Exists(_settings?.SettingsFile))
            {
                _cameraSettings = UICameraSettings.LoadSettingsLocal(_settings.SettingsFile).ToCameraSettings();
                
            }

            _client.ApplySettings(_cameraSettings);
        }

        public Task<Mat> GetImage(CancellationToken token)
        {
            
            _lastImage = null;
            if (_imageAquisitionTaskSource != null && !_imageAquisitionTaskSource.Task.IsCanceled && !_imageAquisitionTaskSource.Task.IsCompleted)
            {
                _imageAquisitionTaskSource.SetCanceled(CancellationToken.None);
                _imageAquisitionTaskSource = null;

            }
            _imageAquisitionTaskSource= new TaskCompletionSource<Mat>();
            if (token.CanBeCanceled)
            {
                token.Register(() =>
                {
                    _imageAquisitionTaskSource.TrySetCanceled(token);
                });
            }
            _client.Trigger();
            return _imageAquisitionTaskSource.Task;

        }

        private TaskCompletionSource<Mat>? _imageAquisitionTaskSource;
        private Mat _lastImage;
        byte[] _flipBuffer = new byte[2000 * 2000];
        private OpenCVBayerProcessor _bayerFilter;

        private void _client_ImageReceived(byte[] obj)
        {
            try
            {
                Log.Debug("Got image on {adapter}", _settings.Adapter);
                Log.Debug("Bytes: {bytes}", obj.Length);
                //FlipLines(obj, _flipBuffer);
                
                var pinnedArray = GCHandle.Alloc(obj, GCHandleType.Pinned);
                var pointer = pinnedArray.AddrOfPinnedObject();

                
                
                Mat image = Mat.FromPixelData(_cameraSettings.ImageSettings.Lines, _cameraSettings.ImageSettings.SensorWidth,
                    MatType.CV_8UC1, pointer);




                var xCrop = _cameraSettings.ImageSettings.SensorWidth - _cameraSettings.ImageWidth -
                            _cameraSettings.OffsetX;

                // crop the image with opencv
                //_lastImage = image[0, _cameraSettings.ImageSettings.Lines, xCrop,
                //    xCrop + _cameraSettings.ImageWidth].Clone();
                _lastImage = image;



                if (_cameraSettings.BayerFilter)
                {
                    _lastImage = _bayerFilter.ProcessBayerImageWithChannelControl(_lastImage);
                }

                // rotate 90 degrees CCW
                Cv2.Rotate(_lastImage, _lastImage, RotateFlags.Rotate90Counterclockwise);

                //_lastImage = _lastImage.CvtColor(ColorConversionCodes.BGR2RGB);

                _imageAquisitionTaskSource!.SetResult(_lastImage);

                pinnedArray.Free();
            }
            catch (Exception ex)
            {
                Log.Error(ex, "Error processing image from Hawkeye camera at {Adapter}", _settings.Adapter);
            }
        }

        Mat BayerFilter(Mat image)
        {
            // Assumes input is a single-channel Bayer pattern image (CV_8UC1)
            // Output is a 3-channel BGR image (CV_8UC3)
            if (image == null || image.Empty())
                throw new ArgumentException("Input image is null or empty.", nameof(image));

            Mat bgrImage = new Mat();
            Cv2.CvtColor(image, bgrImage, ColorConversionCodes.BayerRG2BGR);

            return bgrImage;
        }

        protected void FlipLines(byte[] src, byte[] dst)
        {
            int srcStride = _cameraSettings.ImageSettings.SensorWidth;
            int dstStride = _cameraSettings.ImageSettings.SensorWidth;
            int pixelSize = 1;
            int copySize = srcStride * pixelSize;
            var lines = _cameraSettings.ImageSettings.Lines;
            for (int line = 0; line < lines; line += 4)
            {
                for (int i = 0; i < 4; i++)
                {
                    int srcPos = (line + i) * srcStride;
                    int dstPos = (line + (4 - i)) * dstStride;
                    for (int j = 0; j < copySize; j++)
                    {
                        dst[dstPos + j] = src[srcPos + j];
                    }


                }
            }
        }

        public void InitializeModule()
        {
            Open();
        }
    }
}