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
    {

        

        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;

        ProcessStartInfo psi = new ProcessStartInfo
        {
            FileName = "libcamera-vid",
            Arguments = "--codec mjpeg -t0 --width 2028 --height 1520 --shutter 10000 --framerate 16 --awbgains 3.86,1.46 --nopreview -o -",

            RedirectStandardOutput = true,
            UseShellExecute = false
        };

        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()
        {
            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();

            }
        }

        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);

        }

    }
}