using System.Runtime.InteropServices;
using Microsoft.Extensions.Logging;
using LibCameraSharp;
using Stream = LibCameraSharp.Stream;
using System.Buffers;
using GSS2.Core.Extentions;

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 = "",
            ViewFinderFrameBufferCount = 5,
            ViewFinderWidth = 1920,
            ViewFinderHeight = 1080,
            ViewFinderFps = 30,
            ImageCaptureFrameBufferCount = 2,
            ImageCaptureWidth = 4056,
            ImageCaptureHeight = 3080,
        };

        public string CameraId { get; set; } = "";
        public int ViewFinderFrameBufferCount { get; set; }
        public int ViewFinderWidth { get; set; }
        public int ViewFinderHeight { get; set; }
        public int ViewFinderFps { get; set; }
        public int ImageCaptureFrameBufferCount { 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 StreamConfiguration? _viewFinderStreamConfiguration;
    private StreamConfiguration? _imageCaptureStreamConfiguration;
    private FrameBufferAllocator? _viewFinderFrameBufferAllocator;
    private FrameBufferAllocator? _imageCaptureFrameBufferAllocator;
    private Stream? _viewFinderStream;
    private Stream? _imageCaptureStream;
    private List<MappedBuffer>? _imageCaptureMappedBuffers;
    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:
            CameraId: {_configuration.CameraId}
            ViewFinderFrameBufferCount: {_configuration.ViewFinderFrameBufferCount}
            ViewFinderWidth: {_configuration.ViewFinderWidth}
            ViewFinderHeight: {_configuration.ViewFinderHeight}
            ViewFinderFps: {_configuration.ViewFinderFps}
            ImageCaptureFrameBufferCount: {_configuration.ImageCaptureFrameBufferCount}
            ImageCaptureWidth: {_configuration.ImageCaptureWidth}
            ImageCaptureHeight: {_configuration.ImageCaptureHeight}
        ");

        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);
            ViewFinderFrameBuffersAllocate(effectiveCancellationToken);
            ImageCaptureFrameBuffersAllocate(effectiveCancellationToken);
            CameraStart(effectiveCancellationToken);
            ViewFinderFrameBuffersInitialize(effectiveCancellationToken);
            ImageCaptureFrameBuffersInitialize(effectiveCancellationToken);
            ImageCaptureFrameBuffersMap(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.Raw);

        _viewFinderStreamConfiguration = _cameraConfiguration.First();
        _viewFinderStreamConfiguration.BufferCount = checked((uint)_configuration.ViewFinderFrameBufferCount);
        _viewFinderStreamConfiguration.Size = new Size(
            checked((uint)_configuration.ViewFinderWidth),
            checked((uint)_configuration.ViewFinderHeight)
        );

        _imageCaptureStreamConfiguration = _cameraConfiguration.Skip(1).First();
        _imageCaptureStreamConfiguration.BufferCount = checked((uint)_configuration.ImageCaptureFrameBufferCount);
        _imageCaptureStreamConfiguration.Size = new Size(
            checked((uint)_configuration.ImageCaptureWidth),
            checked((uint)_configuration.ImageCaptureHeight)
        );

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

        _logger.LogDebug(@$"ViewFinder stream configuration:
            Size: {_viewFinderStreamConfiguration.Size.ToString(true)}
            FrameSize: {_viewFinderStreamConfiguration.FrameSize}
            Stride: {_viewFinderStreamConfiguration.Stride}
            ColorSpace: {_viewFinderStreamConfiguration.ColorSpace?.ToString(true)}
            PixelFormat: {_viewFinderStreamConfiguration.PixelFormat?.ToString(true)}");

        _logger.LogDebug(@$"ImageCapture stream configuration:
            Size: {_imageCaptureStreamConfiguration.Size.ToString(true)}
            FrameSize: {_imageCaptureStreamConfiguration.FrameSize}
            Stride: {_imageCaptureStreamConfiguration.Stride}
            ColorSpace: {_imageCaptureStreamConfiguration.ColorSpace?.ToString(true)}
            PixelFormat: {_imageCaptureStreamConfiguration.PixelFormat?.ToString(true)}");

        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 = _viewFinderStreamConfiguration.Stream();
        _imageCaptureStream = _imageCaptureStreamConfiguration.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 ViewFinderFrameBuffersAllocate(CancellationToken cancellationToken)
    {
        if (_camera is null)
            throw new InvalidOperationException("Camera not initialized");
        if (_viewFinderStream is null)
            throw new InvalidOperationException("ViewFinderStream not initialized");

        cancellationToken.ThrowIfCancellationRequested();

        _viewFinderFrameBufferAllocator = new FrameBufferAllocator(_camera);

        cancellationToken.ThrowIfCancellationRequested();

        int result = _viewFinderFrameBufferAllocator.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;
    }
    private void ImageCaptureFrameBuffersAllocate(CancellationToken cancellationToken)
    {
        if (_camera is null)
            throw new InvalidOperationException("Camera not initialized");
        if (_imageCaptureStream is null)
            throw new InvalidOperationException("ImageCaptureStream not initialized");

        cancellationToken.ThrowIfCancellationRequested();

        _imageCaptureFrameBufferAllocator = new FrameBufferAllocator(_camera);

        cancellationToken.ThrowIfCancellationRequested();

        int result = _imageCaptureFrameBufferAllocator.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 ViewFinderFrameBuffersInitialize(CancellationToken cancellationToken)
    {
        if (_camera is null)
            throw new InvalidOperationException("Camera not initialized");
        if (_viewFinderStream is null)
            throw new InvalidOperationException("ViewFinderStream not initialized");
        if (_viewFinderFrameBufferAllocator is null)
            throw new InvalidOperationException("ViewFinderFrameBufferAllocator not initialized");

        cancellationToken.ThrowIfCancellationRequested();

        var buffers = _viewFinderFrameBufferAllocator.Buffers(_viewFinderStream);

        _logger.LogDebug("View finder frame buffers initialization");
        foreach (var buffer in buffers)
        {
            cancellationToken.ThrowIfCancellationRequested();

            var request = _camera.CreateRequest(checked((ulong)DateTimeOffset.UtcNow.ToUnixTimeMilliseconds()));
            var result = request.AddBuffer(_viewFinderStream, buffer);
            if (result < 0)
                throw new Exception("Error while request create");

            var cts = new TaskCompletionSource();
            void RequestCompleted(object? sender, Camera.RequestCompletedEventArgs e)
            {
                if (e.Request.Handle == request.Handle)
                    cts.SetResult();
            }

            _camera.RequestCompleted += RequestCompleted;
            result = _camera.QueueRequest(request);
            if (result < 0)
            {
                _camera.RequestCompleted -= RequestCompleted;
                throw new Exception("Error while request queue");
            }
            cts.Task.WaitAsync(cancellationToken).GetAwaiter().GetResult();
            _camera.RequestCompleted -= RequestCompleted;

            _logger.LogDebug("Request complited: \n\tResult: {} \n\tStatus: {}\n\tCookie: {}\n\tSequence: {}\n\tHasPendingBuffers: {}", result, request.Status, request.Cookie, request.Sequence, request.HasPendingBuffers);
            _logger.LogDebug("Request buffer: \n\tStatus: {} \n\tCookie: {} \n\tSequence: {} \n\tTimestamp: {}", buffer.Metadata.Status, buffer.Cookie, buffer.Metadata.Sequence, buffer.Metadata.Timestamp);
            _logger.LogDebug("Request buffer planes: ");
            foreach (var (i, plane) in buffer.Metadata.Planes.Enumerate())
                _logger.LogDebug("Plane[{}]: bytes used: 0x{:X}", i, plane.BytesUsed);
            foreach (var (i, plane) in buffer.Planes.Enumerate())
                _logger.LogDebug("Plane[{}]: \n\tFd: 0x{:X} \n\tOffset: 0x{:X} \n\tLength: 0x{:X} \n\tKInvalidOffset: 0x{:X}", i, plane.Fd.Get(), plane.Offset, plane.Length, plane.KInvalidOffset);
        }
    }
    private void ImageCaptureFrameBuffersInitialize(CancellationToken cancellationToken)
    {
        if (_camera is null)
            throw new InvalidOperationException("Camera not initialized");
        if (_imageCaptureStream is null)
            throw new InvalidOperationException("ImageCaptureStream not initialized");
        if (_imageCaptureFrameBufferAllocator is null)
            throw new InvalidOperationException("ImageCaptureFrameBufferAllocator not initialized");

        cancellationToken.ThrowIfCancellationRequested();

        var buffers = _imageCaptureFrameBufferAllocator.Buffers(_imageCaptureStream);

        _logger.LogDebug("Image capture frame buffers initialization");
        foreach (var buffer in buffers)
        {
            cancellationToken.ThrowIfCancellationRequested();

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

            var cts = new TaskCompletionSource();
            void RequestCompleted(object? sender, Camera.RequestCompletedEventArgs e)
            {
                if (e.Request.Handle == request.Handle)
                    cts.SetResult();
            }

            _camera.RequestCompleted += RequestCompleted;
            result = _camera.QueueRequest(request);
            if (result < 0)
            {
                _camera.RequestCompleted -= RequestCompleted;
                throw new Exception("Error while request queue");
            }
            cts.Task.WaitAsync(cancellationToken).GetAwaiter().GetResult();
            _camera.RequestCompleted -= RequestCompleted;

            _logger.LogDebug("Request complited: \n\tResult: {} \n\tStatus: {}\n\tCookie: {}\n\tSequence: {}\n\tHasPendingBuffers: {}", result, request.Status, request.Cookie, request.Sequence, request.HasPendingBuffers);
            _logger.LogDebug("Request buffer: \n\tStatus: {} \n\tCookie: {} \n\tSequence: {} \n\tTimestamp: {}", buffer.Metadata.Status, buffer.Cookie, buffer.Metadata.Sequence, buffer.Metadata.Timestamp);
            foreach (var (i, plane) in buffer.Metadata.Planes.Enumerate())
                _logger.LogDebug("Plane[{}]: bytes used: 0x{:X}", i, plane.BytesUsed);
            foreach (var (i, plane) in buffer.Planes.Enumerate())
                _logger.LogDebug("Plane[{}]: \n\tFd: 0x{:X} \n\tOffset: 0x{:X} \n\tLength: 0x{:X} \n\tKInvalidOffset: 0x{:X}", i, plane.Fd.Get(), plane.Offset, plane.Length, plane.KInvalidOffset);
        }
    }
    private void ImageCaptureFrameBuffersMap(CancellationToken cancellationToken)
    {
        if (_imageCaptureStream is null)
            throw new InvalidOperationException("ImageCaptureStream not initialized");
        if (_imageCaptureFrameBufferAllocator is null)
            throw new InvalidOperationException("ImageCaptureFrameBufferAllocator not initialized");

        cancellationToken.ThrowIfCancellationRequested();

        _imageCaptureMappedBuffers = new List<MappedBuffer>();

        var frameBuffers = _imageCaptureFrameBufferAllocator.Buffers(_imageCaptureStream);

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

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

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

                int fd = fdGroup.Key;

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

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

                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=0x{fd:X}, error={error}");
                }

                _logger.LogDebug("Mapped buffer fd=0x{:X}, address=0x{:X}, length=0x{:X}", fd, address, alignedLength);

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

        _state |= State.ImageCaptureFrameBuffersMapped;
    }

    public async Task<OpenCvSharp.Mat?> CaptureImage(CancellationToken cancellationToken, int framesCount = 1)
    {
        ArgumentOutOfRangeException.ThrowIfLessThan(framesCount, 1, nameof(framesCount));

        if (_camera is null)
            throw new InvalidOperationException("Camera not initialized");
        if (_imageCaptureStream is null)
            throw new InvalidOperationException("ImageCaptureStream not initialized");
        if (_imageCaptureStreamConfiguration is null)
            throw new InvalidOperationException("ImageCaptureStreamConfiguration not initialized");
        if (_imageCaptureFrameBufferAllocator is null)
            throw new InvalidOperationException("ImageCaptureFrameBufferAllocator not initialized");
        if (_imageCaptureMappedBuffers is null)
            throw new InvalidOperationException("ImageCaptureMappedBuffers not mapped");

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

        _logger.LogDebug("Capturing {} frame(s)", framesCount);

        OpenCvSharp.Mat? image = null;

        var planesData = new List<byte[]>();
        var fdsData = new Dictionary<int, byte[]>();

        try
        {
            while (planesData.Count() < framesCount)
            {
                effectiveCancellationToken.ThrowIfCancellationRequested();
                _logger.LogDebug("Requested {} frame(s), now collected {}", framesCount, planesData.Count());
                if (File.Exists("/proc/self/status"))
                    _logger.LogDebug("Current RAM Usage: {}", File.ReadAllLines("/proc/self/status").FirstOrDefault(l => l.StartsWith("RssAnon:"))?.Substring("RssAnon:".Length)?.Trim() ?? "UNKNOWN");

                var oldPlanesDataCount = planesData.Count();

                var buffers = _imageCaptureFrameBufferAllocator.Buffers(_imageCaptureStream);
                if (buffers.Count() > framesCount - planesData.Count())
                    buffers = buffers.Take(framesCount - planesData.Count());
                _logger.LogDebug("Taken {} buffer(s)", buffers.Count());

                var requests = new List<Request>();

                foreach (var buffer in buffers)
                {
                    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");
                    requests.Add(request);
                }
                _logger.LogDebug("Request(s) created");

                effectiveCancellationToken.ThrowIfCancellationRequested();

                var cts = new TaskCompletionSource();
                void RequestCompleted(object? sender, Camera.RequestCompletedEventArgs e)
                {
                    if (!requests.Any(r => r.Handle == e.Request.Handle))
                        return;

                    var buffer = e.Request.FindBuffer(_imageCaptureStream);
                    if (buffer.Metadata.Status == FrameMetadata.StatusEnum.FrameError ||
                        buffer.Metadata.Status == FrameMetadata.StatusEnum.FrameStartup)
                    {
                        _logger.LogDebug("Request buffers status: {}", buffer.Metadata.Status);
                        e.Request.Reuse(Request.ReuseFlagEnum.ReuseBuffers);
                        var result = _camera.QueueRequest(e.Request);
                        if (result >= 0)
                            return;
                    }

                    _logger.LogDebug("Request complited: \n\tStatus: {}\n\tCookie: {}\n\tSequence: {}\n\tHasPendingBuffers: {}", e.Request.Status, e.Request.Cookie, e.Request.Sequence, e.Request.HasPendingBuffers);
                    _logger.LogDebug("Request buffer: \n\tStatus: {} \n\tCookie: {} \n\tSequence: {} \n\tTimestamp: {}", buffer.Metadata.Status, buffer.Cookie, buffer.Metadata.Sequence, buffer.Metadata.Timestamp);
                    _logger.LogDebug("Request buffer planes: ");
                    foreach (var (i, plane) in buffer.Metadata.Planes.Enumerate())
                        _logger.LogDebug("Plane[{}]: bytes used: 0x{:X}", i, plane.BytesUsed);
                    foreach (var (i, plane) in buffer.Planes.Enumerate())
                        _logger.LogDebug("Plane[{}]: \n\tFd: 0x{:X} \n\tOffset: 0x{:X} \n\tLength: 0x{:X} \n\tKInvalidOffset: 0x{:X}", i, plane.Fd.Get(), plane.Offset, plane.Length, plane.KInvalidOffset);

                    effectiveCancellationToken.ThrowIfCancellationRequested();

                    foreach (var plane in buffer.Planes)
                    {
                        var fd = plane.Fd.Get();
                        if (fdsData.ContainsKey(fd))
                            continue;
                        var mappedBuffer = _imageCaptureMappedBuffers.Find(b => b.Fd == fd);
                        if (mappedBuffer is null)
                            continue;
                        var fdData = new byte[mappedBuffer.Length];
                        Marshal.Copy(mappedBuffer.Address, fdData, 0, checked((int)mappedBuffer.Length));
                        fdsData.Add(fd, fdData);
                    }

                    foreach (var plane in buffer.Planes)
                    {
                        var fd = plane.Fd.Get();
                        if (!fdsData.ContainsKey(fd))
                            continue;
                        var fdData = fdsData[fd];
                        var planeData = fdData
                            .Skip(checked((int)plane.Offset))
                            .Take(checked((int)plane.Length))
                            .ToArray();
                        planesData.Add(planeData);
                    }

                    if (planesData.Count() - oldPlanesDataCount == requests.Count())
                        cts.SetResult();
                }

                _camera.RequestCompleted += RequestCompleted;
                foreach (var request in requests)
                {
                    var result = _camera.QueueRequest(request);
                    if (result < 0)
                    {
                        _camera.RequestCompleted -= RequestCompleted;
                        throw new Exception("Error while request queue");
                    }
                }
                _logger.LogDebug("Request(s) queued");
                await cts.Task.WaitAsync(effectiveCancellationToken);
                _camera.RequestCompleted -= RequestCompleted;

                foreach (var key in fdsData.Keys)
#pragma warning disable CS8625 // Cannot convert null literal to non-nullable reference type.
                    fdsData[key] = null;
#pragma warning restore CS8625 // Cannot convert null literal to non-nullable reference type.
                fdsData.Clear();
            }

            _logger.LogDebug("Frame(s) collected");
            if (File.Exists("/proc/self/status"))
                _logger.LogDebug("Current RAM Usage: {}", File.ReadAllLines("/proc/self/status").FirstOrDefault(l => l.StartsWith("RssAnon:"))?.Substring("RssAnon:".Length)?.Trim() ?? "UNKNOWN");


            effectiveCancellationToken.ThrowIfCancellationRequested();

            var config = _imageCaptureStreamConfiguration;
            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=0x{fourcc:X}({new string(BitConverter.GetBytes(fourcc).Select(b => (char)b).ToArray())}) modifier=0x{modifier:X} primariesEnum={primariesEnum} transferFunctionEnum={transferFunctionEnum} ycbcrEncodingEnum={ycbcrEncodingEnum} rangeEnum={rangeEnum}");
            _logger.LogDebug("Found decoder {}", decoder);

            effectiveCancellationToken.ThrowIfCancellationRequested();

            try
            {
                image = decoder.Decode(checked((int)size.Width), checked((int)size.Height), checked((int)stride), planesData);
            }
            catch (Exception ex)
            {
                _logger.LogError(ex, "Error while image decode");
                throw;
            }
            _logger.LogDebug("Image decoded successfully");
        }
        catch (Exception e)
        {
            _logger.LogError(e, "Error while image capture");
        }
        finally
        {
            foreach (var key in fdsData.Keys)
#pragma warning disable CS8625 // Cannot convert null literal to non-nullable reference type.
                fdsData[key] = null;
#pragma warning restore CS8625 // Cannot convert null literal to non-nullable reference type.
            fdsData.Clear();
            for (int i = 0; i < planesData.Count(); i++)
#pragma warning disable CS8625 // Cannot convert null literal to non-nullable reference type.
                planesData[i] = null;
#pragma warning restore CS8625 // Cannot convert null literal to non-nullable reference type.
            planesData.Clear();
        }
        return image;
    }

    public async Task<(FrameBuffer, StreamConfiguration)?> CaptureViewFinderBuffer(int bufferId, CancellationToken cancellationToken)
    {
        if (_camera is null)
            throw new InvalidOperationException("Camera not initialized");
        if (_viewFinderStream is null)
            throw new InvalidOperationException("ViewFinderStream not initialized");
        if (_viewFinderStreamConfiguration is null)
            throw new InvalidOperationException("ViewFinderStreamConfiguration not initialized");
        if (_viewFinderFrameBufferAllocator is null)
            throw new InvalidOperationException("ViewFinderFrameBufferAllocator not initialized");

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

        var buffers = _viewFinderFrameBufferAllocator.Buffers(_viewFinderStream);
        var buffer = buffers.Skip(bufferId % buffers.Count()).First();

        var request = _camera.CreateRequest(checked((ulong)DateTimeOffset.UtcNow.ToUnixTimeMilliseconds()));
        var result = request.AddBuffer(_viewFinderStream, buffer);
        if (result < 0)
            throw new Exception("Error while request create");
        // _logger.LogDebug("Request(s) created");

        effectiveCancellationToken.ThrowIfCancellationRequested();

        var cts = new TaskCompletionSource();
        void RequestCompleted(object? sender, Camera.RequestCompletedEventArgs e)
        {
            if (request.Handle != e.Request.Handle)
                return;

            var buffer = e.Request.FindBuffer(_viewFinderStream);
            if (buffer.Metadata.Status == FrameMetadata.StatusEnum.FrameError ||
                buffer.Metadata.Status == FrameMetadata.StatusEnum.FrameStartup)
            {
                // _logger.LogDebug("Request buffers status: {}", buffer.Metadata.Status);
                e.Request.Reuse(Request.ReuseFlagEnum.ReuseBuffers);
                var result = _camera.QueueRequest(e.Request);
                if (result >= 0)
                    return;
            }

            // _logger.LogDebug("Request complited: \n\tStatus: {}\n\tCookie: {}\n\tSequence: {}\n\tHasPendingBuffers: {}", e.Request.Status, e.Request.Cookie, e.Request.Sequence, e.Request.HasPendingBuffers);
            // _logger.LogDebug("Request buffer: \n\tStatus: {} \n\tCookie: {} \n\tSequence: {} \n\tTimestamp: {}", buffer.Metadata.Status, buffer.Cookie, buffer.Metadata.Sequence, buffer.Metadata.Timestamp);
            // _logger.LogDebug("Request buffer planes: ");
            // foreach (var (i, plane) in buffer.Metadata.Planes.Enumerate())
            //     _logger.LogDebug("Plane[{}]: bytes used: 0x{:X}", i, plane.BytesUsed);
            // foreach (var (i, plane) in buffer.Planes.Enumerate())
            //     _logger.LogDebug("Plane[{}]: \n\tFd: 0x{:X} \n\tOffset: 0x{:X} \n\tLength: 0x{:X} \n\tKInvalidOffset: 0x{:X}", i, plane.Fd.Get(), plane.Offset, plane.Length, plane.KInvalidOffset);

            effectiveCancellationToken.ThrowIfCancellationRequested();

            cts.SetResult();
        }

        _camera.RequestCompleted += RequestCompleted;
        result = _camera.QueueRequest(request);
        if (result < 0)
        {
            _camera.RequestCompleted -= RequestCompleted;
            throw new Exception("Error while request queue");
        }
        // _logger.LogDebug("Request queued");
        await cts.Task.WaitAsync(effectiveCancellationToken);
        _camera.RequestCompleted -= RequestCompleted;
        // _logger.LogDebug("Request complited");

        return new(buffer, _viewFinderStreamConfiguration);
    }

    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) && _imageCaptureMappedBuffers is not null)
            {
                foreach (var buffer in _imageCaptureMappedBuffers)
                {
                    _logger.LogDebug("MUNMAP address=0x{:X}, length=0x{:X}", buffer.Address, buffer.Length);
                    LibC.munmap(buffer.Address, buffer.Length);
                }
                _imageCaptureMappedBuffers.Clear();
            }
            if (_state.HasFlag(State.ViewFinderFrameBufferAllocated) && _viewFinderStream is not null)
                _viewFinderFrameBufferAllocator?.Free(_viewFinderStream);
            if (_state.HasFlag(State.ImageCaptureFrameBufferAllocated) && _imageCaptureStream is not null)
                _imageCaptureFrameBufferAllocator?.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();
            // _viewFinderFrameBufferAllocator?.Dispose();
            // _imageCaptureFrameBufferAllocator?.Dispose();
            _cameraManager?.Dispose();
        }
        catch (Exception e)
        {
            _logger.LogError(e, "Error while CameraService cleanup");
        }
        finally
        {
            _cameraManager = null;
            _camera = null;
            _viewFinderFrameBufferAllocator = null;
            _imageCaptureFrameBufferAllocator = null;
            _viewFinderStream = null;
            _imageCaptureStream = null;
            _imageCaptureMappedBuffers = null;
            _state = State.NotInitialized;
        }
    }
}