HawkeyeVision/VisionBuilder.UI.RaspberryAcquisition/Services/ConfigurableLibcameraSource.cs

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

namespace VisionBuilder.UI.RaspberryAcquisition.Services;

public class ConfigurableLibcameraSource : IImageSource, IDisposable
{
    private static readonly byte[] JpegHeader = [0xff, 0xd8];
    private static readonly byte[] JpegFooter = [0xff, 0xd9];
    private const int ChunkSize = 1024;

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

    private CaptureSettings _settings;
    private CancellationTokenSource? _cts;
    private Thread? _captureThread;

    public int FrameCount { get; private set; }
    public event Action<string>? StatusChanged;

    public ConfigurableLibcameraSource(CaptureSettings settings)
    {
        _settings = settings;
    }

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

    public void Start()
    {
        Stop();
        FrameCount = 0;
        _cts = new CancellationTokenSource();
        _captureThread = new Thread(CaptureLoop) { IsBackground = true };
        _captureThread.Start();
        StatusChanged?.Invoke("Running");
    }

    public void Stop()
    {
        _cts?.Cancel();
        _captureThread?.Join(2000);
        _cts?.Dispose();
        _cts = null;
        _captureThread = null;
        StatusChanged?.Invoke("Stopped");
    }

    public void UpdateSettings(CaptureSettings newSettings)
    {
        _settings = newSettings;
        Start();
    }

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

        Process? process = null;
        try
        {
            process = Process.Start(psi);
            if (process == null)
            {
                StatusChanged?.Invoke("Error: failed to start rpicam-vid");
                return;
            }

            using var br = new BinaryReader(process.StandardOutput.BaseStream);
            var imageBuffer = new byte[1024 * 1024];
            var buff = br.ReadBytes(ChunkSize);

            while (!_cts!.Token.IsCancellationRequested)
            {
                var imageStart = Find(buff, JpegHeader);
                if (imageStart == -1)
                {
                    StatusChanged?.Invoke("Error: JPEG header not found");
                    break;
                }

                var size = buff.Length - imageStart;
                Array.Copy(buff, imageStart, imageBuffer, 0, size);

                while (!_cts.Token.IsCancellationRequested)
                {
                    buff = br.ReadBytes(ChunkSize);
                    var imageEnd = Find(buff, JpegFooter);
                    if (imageEnd != -1)
                    {
                        imageEnd += JpegFooter.Length;
                        var frame = new byte[size + imageEnd];
                        Array.Copy(imageBuffer, frame, size);
                        Array.Copy(buff, 0, frame, size, imageEnd);

                        var decoded = Mat.ImDecode(frame);
                        FrameCount++;
                        _imageChannel.Writer.TryWrite(decoded);

                        var remaining = buff.Length - imageEnd;
                        var newBuff = new byte[ChunkSize];
                        Array.Copy(buff, imageEnd, newBuff, 0, remaining);
                        var temp = br.ReadBytes(imageEnd);
                        Array.Copy(temp, 0, newBuff, remaining, temp.Length);
                        buff = newBuff;
                        break;
                    }

                    Array.Copy(buff, 0, imageBuffer, size, buff.Length);
                    size += buff.Length;
                }
            }
        }
        catch (Exception ex) when (ex is not OperationCanceledException)
        {
            StatusChanged?.Invoke($"Error: {ex.Message}");
        }
        finally
        {
            if (process is { HasExited: false })
            {
                process.Kill();
                process.Dispose();
            }
        }
    }

    private static int Find(byte[] buff, byte[] search)
    {
        for (int start = 0; start <= buff.Length - search.Length; start++)
        {
            if (buff[start] == search[0])
            {
                int next;
                for (next = 1; next < search.Length; next++)
                {
                    if (buff[start + next] != search[next])
                        break;
                }
                if (next == search.Length)
                    return start;
            }
        }
        return -1;
    }

    public void Dispose()
    {
        Stop();
    }
}