I150/srcs/RobotNet10/RobotApp/Communication/CeresSharp/Advanced/ProblemOptions.cs

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

namespace CeresSharp.Advanced;

/// <summary>
/// Options for configuring a Ceres Problem.
/// </summary>
public sealed class ProblemOptions : IDisposable
{
    private readonly ProblemOptionsHandle _handle;
    private System.Runtime.InteropServices.GCHandle? _evaluationCallbackHandle;
    private System.Runtime.InteropServices.GCHandle? _evaluationNativeCallbackHandle;
    private bool _disposed;

    /// <summary>
    /// Creates a new ProblemOptions instance with default settings.
    /// </summary>
    public ProblemOptions()
    {
        _handle = ProblemOptionsHandle.Create();
    }

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

    /// <summary>
    /// Gets or sets whether the problem owns cost functions.
    /// </summary>
    public bool CostFunctionOwnership
    {
        get => CeresNative.ceres_wrapper_problem_options_get_cost_function_ownership(_handle.DangerousGetHandle()) != 0;
        set => CeresNative.ceres_wrapper_problem_options_set_cost_function_ownership(_handle.DangerousGetHandle(), value ? 1 : 0);
    }

    /// <summary>
    /// Gets or sets whether the problem owns loss functions.
    /// </summary>
    public bool LossFunctionOwnership
    {
        get => CeresNative.ceres_wrapper_problem_options_get_loss_function_ownership(_handle.DangerousGetHandle()) != 0;
        set => CeresNative.ceres_wrapper_problem_options_set_loss_function_ownership(_handle.DangerousGetHandle(), value ? 1 : 0);
    }

    /// <summary>
    /// Gets or sets whether the problem owns manifolds.
    /// </summary>
    public bool ManifoldOwnership
    {
        get => CeresNative.ceres_wrapper_problem_options_get_manifold_ownership(_handle.DangerousGetHandle()) != 0;
        set => CeresNative.ceres_wrapper_problem_options_set_manifold_ownership(_handle.DangerousGetHandle(), value ? 1 : 0);
    }

    /// <summary>
    /// Gets or sets whether to enable fast removal of residual blocks.
    /// </summary>
    public bool EnableFastRemoval
    {
        get => CeresNative.ceres_wrapper_problem_options_get_enable_fast_removal(_handle.DangerousGetHandle()) != 0;
        set => CeresNative.ceres_wrapper_problem_options_set_enable_fast_removal(_handle.DangerousGetHandle(), value ? 1 : 0);
    }

    /// <summary>
    /// Gets or sets whether to disable all safety checks.
    /// </summary>
    public bool DisableAllSafetyChecks
    {
        get => CeresNative.ceres_wrapper_problem_options_get_disable_all_safety_checks(_handle.DangerousGetHandle()) != 0;
        set => CeresNative.ceres_wrapper_problem_options_set_disable_all_safety_checks(_handle.DangerousGetHandle(), value ? 1 : 0);
    }

    /// <summary>
    /// Sets the context for performance optimization.
    /// </summary>
    public void SetContext(Context context)
    {
        if (context == null)
            throw new ArgumentNullException(nameof(context));

        CeresNative.ceres_wrapper_problem_options_set_context(
            _handle.DangerousGetHandle(),
            context.Handle.DangerousGetHandle());
    }

    /// <summary>
    /// Sets an evaluation callback for shared computation.
    /// Called before evaluating cost functions to allow shared computation.
    /// </summary>
    /// <param name="callback">The evaluation callback.</param>
    public void SetEvaluationCallback(EvaluationCallback callback)
    {
        if (callback == null)
            throw new ArgumentNullException(nameof(callback));

        // Pin the callback delegate
        var callbackHandle = System.Runtime.InteropServices.GCHandle.Alloc(callback);

        // Create native callback wrapper
        var nativeCallback = new CeresNative.CeresEvaluationCallback((userData, numResiduals, numParameterBlocks, parameterBlockSizesPtr) =>
        {
            try
            {
                var handle = System.Runtime.InteropServices.GCHandle.FromIntPtr(userData);
                var csCallback = (EvaluationCallback)handle.Target!;

                // Parse parameter block sizes
                int[]? parameterBlockSizes = null;
                if (parameterBlockSizesPtr != IntPtr.Zero && numParameterBlocks > 0)
                {
                    parameterBlockSizes = new int[numParameterBlocks];
                    System.Runtime.InteropServices.Marshal.Copy(parameterBlockSizesPtr, parameterBlockSizes, 0, numParameterBlocks);
                }

                // Call C# callback
                csCallback(numResiduals, numParameterBlocks, parameterBlockSizes);
            }
            catch
            {
                // Ignore errors in callback
            }
        });

        // Pin the native callback
        var nativeCallbackHandle = System.Runtime.InteropServices.GCHandle.Alloc(nativeCallback);

        CeresNative.ceres_wrapper_problem_options_set_evaluation_callback(
            _handle.DangerousGetHandle(),
            nativeCallback,
            System.Runtime.InteropServices.GCHandle.ToIntPtr(callbackHandle));

        // Track handles for cleanup when ProblemOptions is disposed
        _evaluationCallbackHandle = callbackHandle;
        _evaluationNativeCallbackHandle = nativeCallbackHandle;
    }

    public void Dispose()
    {
        if (!_disposed)
        {
            // Cleanup callback handles
            if (_evaluationCallbackHandle.HasValue && _evaluationCallbackHandle.Value.IsAllocated)
                _evaluationCallbackHandle.Value.Free();
            if (_evaluationNativeCallbackHandle.HasValue && _evaluationNativeCallbackHandle.Value.IsAllocated)
                _evaluationNativeCallbackHandle.Value.Free();

            _handle?.Dispose();
            _disposed = true;
        }
    }
}