BQP/srcs/RobotNet10/RobotApp/Communication/CeresSharp/Advanced/Covariance.cs

using System;
using CeresSharp;
using CeresSharp.Native;
using CeresSharp.Native.SafeHandles;

namespace CeresSharp.Advanced;

/// <summary>
/// Covariance estimation for parameter blocks.
/// </summary>
public sealed class Covariance : IDisposable
{
    private readonly CovarianceHandle _handle;
    private bool _disposed;

    /// <summary>
    /// Creates a new Covariance instance with default options.
    /// </summary>
    public Covariance()
    {
        _handle = CovarianceHandle.Create();
    }

    /// <summary>
    /// Creates a new Covariance instance with the specified options.
    /// </summary>
    public Covariance(CovarianceOptions options)
    {
        if (options == null)
            throw new ArgumentNullException(nameof(options));

        _handle = CovarianceHandle.CreateWithOptions(options.Handle.DangerousGetHandle());
    }

    /// <summary>
    /// Gets the native handle.
    /// </summary>
    internal CovarianceHandle Handle => _handle;

    /// <summary>
    /// Computes the covariance for the specified parameter blocks.
    /// </summary>
    /// <param name="problem">The problem.</param>
    /// <param name="options">The covariance options.</param>
    /// <param name="parameterBlocks">Array of parameter block arrays.</param>
    /// <exception cref="Exceptions.CeresException">Thrown if the operation fails.</exception>
    public void Compute(Problem problem, CovarianceOptions options, double[][] parameterBlocks)
    {
        if (problem == null)
            throw new ArgumentNullException(nameof(problem));
        if (options == null)
            throw new ArgumentNullException(nameof(options));
        if (parameterBlocks == null || parameterBlocks.Length == 0)
            throw new ArgumentException("Parameter blocks cannot be null or empty", nameof(parameterBlocks));

        unsafe
        {
            var parameterBlockPtrs = new IntPtr[parameterBlocks.Length];
            var pinnedArrays = new System.Runtime.InteropServices.GCHandle[parameterBlocks.Length];

            try
            {
                for (int i = 0; i < parameterBlocks.Length; i++)
                {
                    if (parameterBlocks[i] == null)
                        throw new ArgumentException($"Parameter block {i} is null", nameof(parameterBlocks));

                    var handle = System.Runtime.InteropServices.GCHandle.Alloc(parameterBlocks[i], System.Runtime.InteropServices.GCHandleType.Pinned);
                    pinnedArrays[i] = handle;
                    parameterBlockPtrs[i] = handle.AddrOfPinnedObject();
                }

                var errorMessage = new System.Text.StringBuilder(512);
                fixed (IntPtr* ptrs = parameterBlockPtrs)
                {
                    var errorCode = CeresNative.ceres_wrapper_covariance_compute(
                        _handle.DangerousGetHandle(),
                        problem.Handle.DangerousGetHandle(),
                        options.Handle.DangerousGetHandle(),
                        (IntPtr)ptrs,
                        parameterBlocks.Length,
                        errorMessage,
                        errorMessage.Capacity);
                    
                    if (errorCode != Exceptions.CeresErrorCode.Success)
                    {
                        var message = errorMessage.Length > 0 ? errorMessage.ToString() : null;
                        throw new Exceptions.CeresException(errorCode, message);
                    }
                }
            }
            finally
            {
                foreach (var handle in pinnedArrays)
                {
                    if (handle.IsAllocated)
                        handle.Free();
                }
            }
        }
    }

    /// <summary>
    /// Gets the covariance block between two parameter blocks.
    /// </summary>
    /// <param name="parameterBlock1">First parameter block.</param>
    /// <param name="parameterBlock2">Second parameter block.</param>
    /// <param name="covarianceBlock">Output covariance block matrix (row-major).</param>
    /// <exception cref="Exceptions.CeresException">Thrown if the operation fails.</exception>
    public void GetCovarianceBlock(double[] parameterBlock1, double[] parameterBlock2, double[] covarianceBlock)
    {
        if (parameterBlock1 == null)
            throw new ArgumentNullException(nameof(parameterBlock1));
        if (parameterBlock2 == null)
            throw new ArgumentNullException(nameof(parameterBlock2));
        if (covarianceBlock == null)
            throw new ArgumentNullException(nameof(covarianceBlock));

        unsafe
        {
            var errorMessage = new System.Text.StringBuilder(512);
            fixed (double* ptr1 = parameterBlock1)
            fixed (double* ptr2 = parameterBlock2)
            fixed (double* covPtr = covarianceBlock)
            {
                var errorCode = CeresNative.ceres_wrapper_covariance_get_covariance_block(
                    _handle.DangerousGetHandle(),
                    (IntPtr)ptr1,
                    (IntPtr)ptr2,
                    (IntPtr)covPtr,
                    errorMessage,
                    errorMessage.Capacity);
                
                if (errorCode != Exceptions.CeresErrorCode.Success)
                {
                    var message = errorMessage.Length > 0 ? errorMessage.ToString() : null;
                    throw new Exceptions.CeresException(errorCode, message);
                }
            }
        }
    }

    /// <summary>
    /// Gets the covariance matrix for multiple parameter blocks.
    /// </summary>
    /// <param name="parameterBlocks">Array of parameter block arrays.</param>
    /// <param name="covarianceMatrix">Output covariance matrix (row-major).</param>
    /// <exception cref="Exceptions.CeresException">Thrown if the operation fails.</exception>
    public void GetCovarianceMatrix(double[][] parameterBlocks, double[] covarianceMatrix)
    {
        if (parameterBlocks == null || parameterBlocks.Length == 0)
            throw new ArgumentException("Parameter blocks cannot be null or empty", nameof(parameterBlocks));
        if (covarianceMatrix == null)
            throw new ArgumentNullException(nameof(covarianceMatrix));

        unsafe
        {
            var parameterBlockPtrs = new IntPtr[parameterBlocks.Length];
            var pinnedArrays = new System.Runtime.InteropServices.GCHandle[parameterBlocks.Length];

            try
            {
                for (int i = 0; i < parameterBlocks.Length; i++)
                {
                    if (parameterBlocks[i] == null)
                        throw new ArgumentException($"Parameter block {i} is null", nameof(parameterBlocks));

                    var handle = System.Runtime.InteropServices.GCHandle.Alloc(parameterBlocks[i], System.Runtime.InteropServices.GCHandleType.Pinned);
                    pinnedArrays[i] = handle;
                    parameterBlockPtrs[i] = handle.AddrOfPinnedObject();
                }

                var errorMessage = new System.Text.StringBuilder(512);
                fixed (IntPtr* ptrs = parameterBlockPtrs)
                fixed (double* covPtr = covarianceMatrix)
                {
                    var errorCode = CeresNative.ceres_wrapper_covariance_get_covariance_matrix(
                        _handle.DangerousGetHandle(),
                        (IntPtr)ptrs,
                        parameterBlocks.Length,
                        (IntPtr)covPtr,
                        errorMessage,
                        errorMessage.Capacity);
                    
                    if (errorCode != Exceptions.CeresErrorCode.Success)
                    {
                        var message = errorMessage.Length > 0 ? errorMessage.ToString() : null;
                        throw new Exceptions.CeresException(errorCode, message);
                    }
                }
            }
            finally
            {
                foreach (var handle in pinnedArrays)
                {
                    if (handle.IsAllocated)
                        handle.Free();
                }
            }
        }
    }

    public void Dispose()
    {
        if (!_disposed)
        {
            _handle?.Dispose();
            _disposed = true;
        }
    }
}