HawkeyeVision/Hawkeye.VisionBuilder.UI.Sources.Raspberry.Libcamera/LibcameraImageSource.cs

using OpenCvSharp;
using System.Diagnostics;
using System.Threading.Channels;
using VisionBuilder.UI.Common;
using VisionBuilder.UI.Common.Processing;

namespace Hawkeye.VisionBuilder.UI.Sources.Raspberry.Libcamera
{
    public class LibcameraImageSource : IImageSource, IVisionBuilderModule
    {
        private LibcameraCaptureSettings _settings;

        public LibcameraImageSource(LibcameraCaptureSettings settings)
        {
            _settings = settings;
        }

        public async Task<Mat> GetImage(CancellationToken token)
        {
            return await _imageChannel.Reader.ReadAsync(token);
        }

        public void InitializeModule()
        {
            Start();
        }

        byte[] JpegHeader = new byte[] { 0xff, 0xd8 };
        byte[] JpegFooter = new byte[] { 0xff, 0xd9 };
        int ChunkSize = 1024;

        Channel<Mat> _imageChannel = Channel.CreateBounded<Mat>(new BoundedChannelOptions(1) { FullMode = BoundedChannelFullMode.DropOldest });

        public void Start()
        {
            Thread th = new Thread(StartLoop);
            th.IsBackground = true;
            th.Start();

        }

        private void StartLoop()
        {
            try
            {
                var psi = new ProcessStartInfo
                {
                    FileName = "rpicam-vid",
                    Arguments = _settings.BuildArguments(),
                    RedirectStandardOutput = true,
                    UseShellExecute = false
                };

                using (Process process = Process.Start(psi))
                using (BinaryReader br = new BinaryReader(process.StandardOutput.BaseStream))
                {

                    byte[] imageBuffer = new byte[1024 * 1024];


                    // decode MJPEG buffer
                    var buff = br.ReadBytes(ChunkSize);
                    int frameId = 0;
                    while (true)
                    {


                        var imageStart = Find(buff, JpegHeader);
                        if (imageStart != -1)
                        {
                            var size = buff.Length - imageStart;

                            Array.Copy(buff, imageStart, imageBuffer, 0, size);
                            while (true)
                            {
                                buff = br.ReadBytes(ChunkSize);
                                var imageEnd = Find(buff, JpegFooter);
                                if (imageEnd != -1)
                                {
                                    var frame = new byte[size + imageEnd];
                                    Array.Copy(imageBuffer, frame, size);
                                    Array.Copy(buff, 0, frame, size, imageEnd);
                                    // process frame
                                    frameId++;
                                    var decoded = Mat.ImDecode(frame);

                                    _imageChannel.Writer.WriteAsync(decoded, CancellationToken.None);

                                    // copy the leftover data to the start
                                    Array.Copy(buff, imageEnd, buff, 0, buff.Length - imageEnd);
                                    // fill the remainder of the buffer with new data and start over
                                    var temp = br.ReadBytes(imageEnd);
                                    Array.Copy(temp, 0, buff, buff.Length - imageEnd, temp.Length);
                                    break;
                                }

                                // copy all of the data to the imageBuffer
                                Array.Copy(buff, 0, imageBuffer, size, buff.Length);
                                size += buff.Length;
                            }
                        }
                        else
                        {
                            Console.WriteLine("JPEG header not found.");
                            break;
                        }

                    }


                    process.Kill();

                }
            }
            catch (Exception e)
            {
                Serilog.Log.Error($"Error starting libcamera: {e}");
            }
        }

        public static int Find(byte[] buff, byte[] search)
        {
            // enumerate the buffer but don't overstep the bounds
            for (int start = 0; start < buff.Length - search.Length; start++)
            {
                // we found the first character
                if (buff[start] == search[0])
                {
                    int next;

                    // traverse the rest of the bytes
                    for (next = 1; next < search.Length; next++)
                    {
                        // if we don't match, bail
                        if (buff[start + next] != search[next])
                            break;
                    }

                    if (next == search.Length)
                        return start;
                }
            }
            // not found
            return -1;
        }

        public async Task<Mat> GetImageAsync(CancellationToken cancellationToken)
        {
            return await _imageChannel.Reader.ReadAsync(cancellationToken);

        }

    }
}