GSS2Rework/GSS2.Core/Hardware/CameraService.cs

using System.Runtime.InteropServices;
using Microsoft.Extensions.Logging;
using LibCameraSharp;
using Stream = LibCameraSharp.Stream;

namespace GSS2.Core.Hardware;

public class CameraService : IDisposable
{
    private static class LibC
    {
        public const int PROT_READ = 0x01;
        public const int MAP_SHARED = 0x0001;

        [DllImport("libc", SetLastError = true)] public static extern IntPtr mmap(IntPtr address, ulong length, int prot, int flags, int fd, ulong offset);
        [DllImport("libc", SetLastError = true)] public static extern int munmap(IntPtr address, ulong length);
    }
    private sealed class MappedBuffer
    {
        public int Fd { get; init; }
        public IntPtr Address { get; init; }
        public ulong Length { get; init; }
        public ulong AlignedOffset { get; init; }
    }

    public class CameraServiceConfiguration
    {
        public static CameraServiceConfiguration Default => new CameraServiceConfiguration
        {
            CameraId = "",
            FrameBufferCount = 5,
            ViewFinderWidth = 1920,
            ViewFinderHeight = 1080,
            ViewFinderFps = 30,
            ImageCaptureWidth = 4056,
            ImageCaptureHeight = 3080,
        };

        public string CameraId { get; set; } = "";
        public int FrameBufferCount { get; set; }
        public int ViewFinderWidth { get; set; }
        public int ViewFinderHeight { get; set; }
        public int ViewFinderFps { get; set; }
        public int ImageCaptureWidth { get; set; }
        public int ImageCaptureHeight { get; set; }
    }

    [Flags]
    private enum State
    {
        NotInitialized = 0,
        CameraManagerStarted = 1 << 1,
        CameraAcquired = 1 << 2,
        CameraStarted = 1 << 3,
        ViewFinderFrameBufferAllocated = 1 << 4,
        ImageCaptureFrameBufferAllocated = 1 << 5,
        ImageCaptureFrameBuffersMapped = 1 << 6
    }

    private bool _disposedValue;
    private readonly ILogger<CameraService> _logger;
    private readonly CameraServiceConfiguration _configuration;
    private State _state = State.NotInitialized;
    private CameraManager? _cameraManager;
    private Camera? _camera;
    private CameraConfiguration? _cameraConfiguration;
    private FrameBufferAllocator? _frameBufferAllocator;
    private Stream? _viewFinderStream;
    private Stream? _imageCaptureStream;
    private List<MappedBuffer>? _mappedBuffers;
    private readonly CancellationTokenSource _disposeCts = new CancellationTokenSource();

    public CameraService(ILogger<CameraService> logger, CameraServiceConfiguration? configuration = null, bool autoInitialize = false)
    {
        _logger = logger;

        _logger.LogInformation("CameraService initialization");

        if (configuration is null)
        {
            _logger.LogWarning("CameraService configuration not set. Using default configuration.");
            _configuration = CameraServiceConfiguration.Default;
        }
        else
        {
            _configuration = configuration;
        }

        _logger.LogDebug(@$"CameraService use following configuration:
        ");

        if (autoInitialize)
            Initialize(CancellationToken.None);
    }

    public async Task InitializeAsync(CancellationToken cancellationToken)
    {
        var linkedCancellationToken = CancellationTokenSource
            .CreateLinkedTokenSource(_disposeCts.Token, cancellationToken)
            .Token;
        await Task.Run(() => Initialize(linkedCancellationToken), linkedCancellationToken);
    }
    public void Initialize(CancellationToken cancellationToken)
    {
        var effectiveCancellationToken = CancellationTokenSource
            .CreateLinkedTokenSource(_disposeCts.Token, cancellationToken)
            .Token;

        try
        {
            if (_state != State.NotInitialized)
                throw new InvalidOperationException("Camera already initialized");

            CameraManagerCreateAndStart(effectiveCancellationToken);
            CameraSelect(effectiveCancellationToken);
            CameraAcquire(effectiveCancellationToken);
            CameraConfigure(effectiveCancellationToken);
            FrameBuffersAllocate(effectiveCancellationToken);
            ImageCaptureFrameBuffersMap(effectiveCancellationToken);
            CameraStart(effectiveCancellationToken);
        }
        catch (Exception e)
        {
            _logger.LogCritical(e, "An exception occurred while camera initialization");
            Cleanup();
            throw;
        }
    }
    private void CameraManagerCreateAndStart(CancellationToken cancellationToken)
    {
        cancellationToken.ThrowIfCancellationRequested();

        _cameraManager = new CameraManager();
        int result = _cameraManager.Start();
        if (result < 0)
        {
            _logger.LogCritical("Error while camera manager start {Result}", result);
            throw new Exception($"Error while camera manager start {result}");
        }

        _state |= State.CameraManagerStarted;
    }
    private void CameraSelect(CancellationToken cancellationToken)
    {
        if (_cameraManager is null)
            throw new InvalidOperationException("CameraManager not initialized");

        cancellationToken.ThrowIfCancellationRequested();

        if (_cameraManager.Cameras.Count() == 0)
            throw new Exception("No cameras found");

        _camera = _cameraManager.Cameras
            .FirstOrDefault(c => c.Id == _configuration.CameraId);

        if (_camera is null)
        {
            var camerasIds = string.Join("\", \"", _cameraManager.Cameras.Select(c => c.Id));
            _logger.LogError(
                "Configured camera id (\"{CameraId}\") not found, using first camera from following list: [\"{Cameras}\"]",
                _configuration.CameraId,
                camerasIds);

            _camera = _cameraManager.Cameras.First();
        }
    }
    private void CameraAcquire(CancellationToken cancellationToken)
    {
        if (_camera is null)
            throw new InvalidOperationException("Camera not initialized");

        cancellationToken.ThrowIfCancellationRequested();

        int result = _camera.Acquire();
        if (result < 0)
        {
            _logger.LogCritical("Error while camera acquire {Result}", result);
            throw new Exception($"Error while camera acquire {result}");
        }

        _state |= State.CameraAcquired;
    }
    private void CameraConfigure(CancellationToken cancellationToken)
    {
        if (_camera is null)
            throw new InvalidOperationException("Camera not initialized");

        cancellationToken.ThrowIfCancellationRequested();

        _cameraConfiguration = _camera.GenerateConfiguration(
            StreamRoleEnum.Viewfinder,
            StreamRoleEnum.StillCapture);

        var viewFinderConfig = _cameraConfiguration.First();
        viewFinderConfig.BufferCount = (uint)_configuration.FrameBufferCount;
        viewFinderConfig.Size = new Size(
            (uint)_configuration.ViewFinderWidth,
            (uint)_configuration.ViewFinderHeight);

        if (_cameraConfiguration.Validate() == CameraConfiguration.Status.Invalid)
        {
            _logger.LogCritical("Error while camera configuration validation");
            throw new Exception("Error while camera configuration validation");
        }

        cancellationToken.ThrowIfCancellationRequested();

        int result = _camera.Configure(_cameraConfiguration);
        if (result < 0)
        {
            _logger.LogCritical("Error while camera configuration {Result}", result);
            throw new Exception($"Error while camera configuration {result}");
        }

        _viewFinderStream = _cameraConfiguration.First().Stream();
        _imageCaptureStream = _cameraConfiguration.Skip(1).First().Stream();
    }
    private void CameraStart(CancellationToken cancellationToken)
    {
        if (_camera is null)
            throw new InvalidOperationException("Camera not initialized");

        cancellationToken.ThrowIfCancellationRequested();

        int result = _camera.Start();
        if (result < 0)
        {
            _logger.LogCritical("Error while camera start {Result}", result);
            throw new Exception($"Error while camera start {result}");
        }

        _state |= State.CameraStarted;
    }
    private void FrameBuffersAllocate(CancellationToken cancellationToken)
    {
        if (_camera is null)
            throw new InvalidOperationException("Camera not initialized");
        if (_viewFinderStream is null)
            throw new InvalidOperationException("ViewFinderStream not initialized");
        if (_imageCaptureStream is null)
            throw new InvalidOperationException("ImageCaptureStream not initialized");

        cancellationToken.ThrowIfCancellationRequested();

        _frameBufferAllocator = new FrameBufferAllocator(_camera);

        cancellationToken.ThrowIfCancellationRequested();

        int result = _frameBufferAllocator.Allocate(_viewFinderStream);
        if (result < 0)
        {
            _logger.LogCritical("Error while view finder frame buffers allocation {Result}", result);
            throw new Exception($"Error while view finder frame buffers allocation {result}");
        }

        _state |= State.ViewFinderFrameBufferAllocated;

        cancellationToken.ThrowIfCancellationRequested();

        result = _frameBufferAllocator.Allocate(_imageCaptureStream);
        if (result < 0)
        {
            _logger.LogCritical("Error while image capture frame buffers allocation {Result}", result);
            throw new Exception($"Error while image capture frame buffers allocation {result}");
        }

        _state |= State.ImageCaptureFrameBufferAllocated;
    }
    private void ImageCaptureFrameBuffersMap(CancellationToken cancellationToken)
    {
        if (_imageCaptureStream is null)
            throw new InvalidOperationException("ImageCaptureStream not initialized");
        if (_frameBufferAllocator is null)
            throw new InvalidOperationException("FrameBufferAllocator not initialized");

        cancellationToken.ThrowIfCancellationRequested();

        _mappedBuffers = new List<MappedBuffer>();

        var frameBuffers = _frameBufferAllocator.Buffers(_imageCaptureStream);

        foreach (var buffer in frameBuffers)
        {
            cancellationToken.ThrowIfCancellationRequested();

            var planesByFd = buffer.Planes.GroupBy(p => p.Fd);

            foreach (var fdGroup in planesByFd)
            {
                cancellationToken.ThrowIfCancellationRequested();

                int fd = fdGroup.Key.Get();

                ulong minOffset = fdGroup.Min(p => p.Offset);
                ulong maxEnd = fdGroup.Max(p => p.Offset + p.Length);

                ulong pageSize = (ulong)Environment.SystemPageSize;
                ulong alignedOffset = minOffset & ~(pageSize - 1);
                ulong alignedLength = maxEnd - alignedOffset;

                IntPtr address = LibC.mmap(IntPtr.Zero, alignedLength, LibC.PROT_READ, LibC.MAP_SHARED, fd, alignedOffset);

                if (address == new IntPtr(-1))
                {
                    int error = Marshal.GetLastWin32Error();
                    throw new InvalidOperationException($"Error while buffers mapping, mmap failed for fd={fd}, error={error}");
                }

                _mappedBuffers.Add(new MappedBuffer
                {
                    Fd = fd,
                    Address = address,
                    Length = alignedLength,
                    AlignedOffset = alignedOffset
                });
            }
        }

        _state |= State.ImageCaptureFrameBuffersMapped;
    }

    public async Task<OpenCvSharp.Mat?> CaptureImage(CancellationToken cancellationToken)
    {
        if (_camera is null)
            throw new InvalidOperationException("Camera not initialized");
        if (_imageCaptureStream is null)
            throw new InvalidOperationException("ImageCaptureStream not initialized");
        if (_frameBufferAllocator is null)
            throw new InvalidOperationException("FrameBufferAllocator not initialized");
        if (_mappedBuffers is null)
            throw new InvalidOperationException("Buffers not mapped");

        var effectiveCancellationToken = CancellationTokenSource
            .CreateLinkedTokenSource(_disposeCts.Token, cancellationToken)
            .Token;

        OpenCvSharp.Mat? image = null;

        try
        {
            effectiveCancellationToken.ThrowIfCancellationRequested();

            var buffers = _frameBufferAllocator.Buffers(_imageCaptureStream);
            var buffer = buffers.Where(b => b.Metadata.Status == FrameMetadata.StatusEnum.FrameStartup || b.Metadata.Status == FrameMetadata.StatusEnum.FrameSuccess).First();
            var request = _camera.CreateRequest(checked((ulong)DateTimeOffset.UtcNow.ToUnixTimeMilliseconds()));
            var result = request.AddBuffer(_imageCaptureStream, buffer);

            if (result < 0)
                throw new Exception("Error while request create");

            effectiveCancellationToken.ThrowIfCancellationRequested();

            var cts = new TaskCompletionSource();
            void RequestCompleted(object? sender, Camera.RequestCompletedEventArgs e)
            {
                if (e.Request.Handle == request.Handle)
                    cts.SetResult();
            };
            
            _camera.RequestCompleted += RequestCompleted;
            await cts.Task.WaitAsync(effectiveCancellationToken);
            _camera.RequestCompleted -= RequestCompleted;

            var planesData = new List<Memory<byte>>();
            foreach (var plane in buffer.Planes)
            {
                effectiveCancellationToken.ThrowIfCancellationRequested();

                var pointer = GetPlanePointer(plane);
                var data = new byte[plane.Length];
                Marshal.Copy(pointer, data, 0, checked((int)plane.Length));
                planesData.Add(new Memory<byte>(data));
            }

            effectiveCancellationToken.ThrowIfCancellationRequested();

            var config = _imageCaptureStream.Configuration();
            var size = config.Size;
            var fourcc = config.PixelFormat.Fourcc;
            var modifier = config.PixelFormat.Modifier;
            var primariesEnum = config.ColorSpace!.Primaries;
            var transferFunctionEnum = config.ColorSpace.TransferFunction;
            var ycbcrEncodingEnum = config.ColorSpace.YcbcrEncoding;
            var rangeEnum = config.ColorSpace.Range;
            var stride = config.Stride;

            effectiveCancellationToken.ThrowIfCancellationRequested();

            var decoder = FrameDecoderHelper.FindDecoder(fourcc, modifier, primariesEnum, transferFunctionEnum, ycbcrEncodingEnum, rangeEnum);
            if (decoder is null)
                throw new Exception($"Cannot find decoder for fourcc={fourcc} modifier={modifier} primariesEnum={primariesEnum} transferFunctionEnum={transferFunctionEnum} ycbcrEncodingEnum={ycbcrEncodingEnum} rangeEnum={rangeEnum}");

            effectiveCancellationToken.ThrowIfCancellationRequested();

            try
            {
                image = decoder.Decode(checked((int)size.Width), checked((int)size.Height), checked((int)stride), planesData);
            }
            catch (Exception e)
            {
                _logger.LogError(e, "Error while image decode");
                throw;
            }
        }
        catch (Exception e)
        {
            _logger.LogError(e, "Error while image capture");
        }
        return image;
    }
    private IntPtr GetPlanePointer(FrameBuffer.Plane plane)
    {
        if (_mappedBuffers is null)
            throw new InvalidOperationException("Buffers not mapped");

        var mapped = _mappedBuffers.First(m => m.Fd == plane.Fd.Get());
        ulong offsetInMapping = plane.Offset - mapped.AlignedOffset;
        return IntPtr.Add(mapped.Address, checked((int)offsetInMapping));
    }

    protected virtual void Dispose(bool disposing)
    {
        if (!_disposedValue)
        {
            if (disposing)
            {
                _disposeCts.Cancel();
                Cleanup();
            }
            _disposedValue = true;
        }
    }
    void IDisposable.Dispose()
    {
        Dispose(disposing: true);
        GC.SuppressFinalize(this);
    }
    private void Cleanup()
    {
        try
        {
            if (_state.HasFlag(State.ImageCaptureFrameBuffersMapped) && _mappedBuffers is not null)
            {
                foreach (var buffer in _mappedBuffers)
                    LibC.munmap(buffer.Address, buffer.Length);
                _mappedBuffers.Clear();
            }
            if (_state.HasFlag(State.ViewFinderFrameBufferAllocated) && _viewFinderStream is not null)
                _frameBufferAllocator?.Free(_viewFinderStream);
            if (_state.HasFlag(State.ImageCaptureFrameBufferAllocated) && _imageCaptureStream is not null)
                _frameBufferAllocator?.Free(_imageCaptureStream);
            if (_state.HasFlag(State.CameraStarted))
                _camera?.Stop();
            if (_state.HasFlag(State.CameraAcquired))
                _camera?.Release();
            if (_state.HasFlag(State.CameraManagerStarted))
                _cameraManager?.Stop();

            _viewFinderStream?.Dispose();
            _imageCaptureStream?.Dispose();
            _frameBufferAllocator?.Dispose();
            _cameraManager?.Dispose();
        }
        catch (Exception e)
        {
            _logger.LogError(e, "Error while CameraService cleanup");
        }
        finally
        {
            _cameraManager = null;
            _camera = null;
            _frameBufferAllocator = null;
            _viewFinderStream = null;
            _imageCaptureStream = null;
            _mappedBuffers = null;
            _state = State.NotInitialized;
        }
    }
}