/* Ceres Wrapper - Extended C API Implementation
 * Copyright 2024 RobotNet10. All rights reserved.
 *
 * This implementation provides additional C APIs that are missing from the
 * official Ceres C API but are required for Cartographer integration.
 */

#include "ceres_wrapper.h"

#include <cstring>
#include <memory>
#include <string>
#include <vector>
#include <algorithm>
#include <functional>
#include <map>
#include <cstdio>
#include <cmath>

#include "ceres/ceres.h"
#include "ceres/cubic_interpolation.h"
#include "ceres/manifold.h"
#include "ceres/problem.h"
#include "ceres/solver.h"
#include "ceres/sphere_manifold.h"
#include "ceres/line_manifold.h"
#include "ceres/dynamic_autodiff_cost_function.h"
#include "ceres/product_manifold.h"
#include "ceres/iteration_callback.h"
#include "ceres/numeric_diff_cost_function.h"
#include "ceres/dynamic_numeric_diff_cost_function.h"
#include "ceres/covariance.h"
#include "ceres/gradient_checker.h"
#include "ceres/context.h"
#include "ceres/loss_function.h"
#include "ceres/autodiff_manifold.h"
#include "ceres/version.h"
#include <limits>
#include <mutex>
#include <sstream>

// ============================================================================
// Error Code Utilities
// ============================================================================

const char* ceres_wrapper_get_error_message(ceres_wrapper_error_code_t error_code) {
  switch (error_code) {
    case CERES_WRAPPER_SUCCESS:
      return "Success";
    case CERES_WRAPPER_ERROR_NULL_POINTER:
      return "Null pointer argument";
    case CERES_WRAPPER_ERROR_INVALID_PARAMETER:
      return "Invalid parameter value";
    case CERES_WRAPPER_ERROR_INVALID_ENUM:
      return "Invalid enum value";
    case CERES_WRAPPER_ERROR_EXCEPTION:
      return "C++ exception occurred";
    case CERES_WRAPPER_ERROR_OUT_OF_MEMORY:
      return "Memory allocation failed";
    case CERES_WRAPPER_ERROR_INVALID_OPERATION:
      return "Invalid operation for current state";
    case CERES_WRAPPER_ERROR_BUFFER_TOO_SMALL:
      return "Output buffer too small";
    case CERES_WRAPPER_ERROR_NOT_FOUND:
      return "Resource not found";
    case CERES_WRAPPER_ERROR_ALREADY_EXISTS:
      return "Resource already exists";
    default:
      return "Unknown error";
  }
}

// ============================================================================
// Library Information and Version
// ============================================================================

const char* ceres_wrapper_get_version_string() {
  return CERES_VERSION_STRING;
}

int ceres_wrapper_get_version_major() {
  return CERES_VERSION_MAJOR;
}

int ceres_wrapper_get_version_minor() {
  return CERES_VERSION_MINOR;
}

int ceres_wrapper_get_version_revision() {
  return CERES_VERSION_REVISION;
}

ceres_wrapper_error_code_t ceres_wrapper_get_detailed_version_string(
    char* buffer,
    int buffer_size) {
  if (!buffer || buffer_size <= 0) {
    return CERES_WRAPPER_ERROR_NULL_POINTER;
  }

  try {
    std::ostringstream oss;
    oss << CERES_VERSION_STRING;
    
    // Add Eigen version
    #if defined(EIGEN_WORLD_VERSION) && defined(EIGEN_MAJOR_VERSION) && defined(EIGEN_MINOR_VERSION)
    oss << "-eigen-(" 
        << EIGEN_WORLD_VERSION << "." 
        << EIGEN_MAJOR_VERSION << "." 
        << EIGEN_MINOR_VERSION << ")";
    #else
    oss << "-eigen-(unknown)";
    #endif
    
    // Add LAPACK info
    #ifdef CERES_NO_LAPACK
    oss << "-no_lapack";
    #else
    oss << "-lapack";
    #endif
    
    // Add SuiteSparse info
    #ifndef CERES_NO_SUITESPARSE
    #ifdef CERES_SUITESPARSE_VERSION
    oss << "-suitesparse-(" << CERES_SUITESPARSE_VERSION << ")";
    #endif
    #endif
    
    // Add METIS info
    #if !defined(CERES_NO_EIGEN_METIS) || !defined(CERES_NO_CHOLMOD_PARTITION)
    #ifdef CERES_METIS_VERSION
    oss << "-metis-(" << CERES_METIS_VERSION << ")";
    #endif
    #endif
    
    // Add AccelerateSparse info
    #ifndef CERES_NO_ACCELERATE_SPARSE
    oss << "-acceleratesparse";
    #endif
    
    // Add EigenSparse info
    #ifdef CERES_USE_EIGEN_SPARSE
    oss << "-eigensparse";
    #endif
    
    // Add Schur specializations info
    #ifdef CERES_RESTRUCT_SCHUR_SPECIALIZATIONS
    oss << "-no_schur_specializations";
    #endif
    
    // Add custom BLAS info
    #ifdef CERES_NO_CUSTOM_BLAS
    oss << "-no_custom_blas";
    #endif
    
    // Add CUDA info
    #ifndef CERES_NO_CUDA
    #ifdef CUDART_VERSION
    oss << "-cuda-(" << CUDART_VERSION << ")";
    #endif
    #endif
    
    std::string version_str = oss.str();
    int copy_size = std::min(static_cast<int>(version_str.size()), buffer_size - 1);
    std::strncpy(buffer, version_str.c_str(), copy_size);
    buffer[copy_size] = '\0';
    
    return CERES_WRAPPER_SUCCESS;
  } catch (...) {
    return CERES_WRAPPER_ERROR_EXCEPTION;
  }
}

ceres_wrapper_error_code_t ceres_wrapper_get_library_info(
    char* buffer,
    int buffer_size) {
  if (!buffer || buffer_size <= 0) {
    return CERES_WRAPPER_ERROR_NULL_POINTER;
  }

  try {
    std::ostringstream oss;
    
    oss << "Ceres Solver Library Information\n";
    oss << "================================\n\n";
    
    // Version information
    oss << "Version: " << CERES_VERSION_STRING << "\n";
    oss << "  Major: " << CERES_VERSION_MAJOR << "\n";
    oss << "  Minor: " << CERES_VERSION_MINOR << "\n";
    oss << "  Revision: " << CERES_VERSION_REVISION << "\n\n";
    
    // Build configuration
    oss << "Build Configuration:\n";
    
    // Eigen version
    #if defined(EIGEN_WORLD_VERSION) && defined(EIGEN_MAJOR_VERSION) && defined(EIGEN_MINOR_VERSION)
    oss << "  Eigen: " 
        << EIGEN_WORLD_VERSION << "." 
        << EIGEN_MAJOR_VERSION << "." 
        << EIGEN_MINOR_VERSION << "\n";
    #else
    oss << "  Eigen: Unknown\n";
    #endif
    
    // LAPACK
    #ifdef CERES_NO_LAPACK
    oss << "  LAPACK: Disabled\n";
    #else
    oss << "  LAPACK: Enabled\n";
    #endif
    
    // SuiteSparse
    #ifdef CERES_NO_SUITESPARSE
    oss << "  SuiteSparse: Disabled\n";
    #else
    oss << "  SuiteSparse: Enabled";
    #ifdef CERES_SUITESPARSE_VERSION
    oss << " (version " << CERES_SUITESPARSE_VERSION << ")";
    #endif
    oss << "\n";
    #endif
    
    // METIS
    #if defined(CERES_NO_EIGEN_METIS) && defined(CERES_NO_CHOLMOD_PARTITION)
    oss << "  METIS: Disabled\n";
    #else
    oss << "  METIS: Enabled";
    #ifdef CERES_METIS_VERSION
    oss << " (version " << CERES_METIS_VERSION << ")";
    #endif
    oss << "\n";
    #endif
    
    // AccelerateSparse
    #ifdef CERES_NO_ACCELERATE_SPARSE
    oss << "  AccelerateSparse: Disabled\n";
    #else
    oss << "  AccelerateSparse: Enabled\n";
    #endif
    
    // EigenSparse
    #ifdef CERES_USE_EIGEN_SPARSE
    oss << "  EigenSparse: Enabled\n";
    #else
    oss << "  EigenSparse: Disabled\n";
    #endif
    
    // Schur specializations
    #ifdef CERES_RESTRUCT_SCHUR_SPECIALIZATIONS
    oss << "  Schur Specializations: Disabled\n";
    #else
    oss << "  Schur Specializations: Enabled\n";
    #endif
    
    // Custom BLAS
    #ifdef CERES_NO_CUSTOM_BLAS
    oss << "  Custom BLAS: Disabled\n";
    #else
    oss << "  Custom BLAS: Enabled\n";
    #endif
    
    // CUDA
    #ifdef CERES_NO_CUDA
    oss << "  CUDA: Disabled\n";
    #else
    oss << "  CUDA: Enabled";
    #ifdef CUDART_VERSION
    oss << " (version " << CUDART_VERSION << ")";
    #endif
    oss << "\n";
    #endif
    
    std::string info_str = oss.str();
    int copy_size = std::min(static_cast<int>(info_str.size()), buffer_size - 1);
    std::strncpy(buffer, info_str.c_str(), copy_size);
    buffer[copy_size] = '\0';
    
    return CERES_WRAPPER_SUCCESS;
  } catch (...) {
    return CERES_WRAPPER_ERROR_EXCEPTION;
  }
}

// ============================================================================
// Forward declarations - Define structs early for use in classes
// ============================================================================

// Define ceres_solver_summary_s early so it can be used in IterationCallbackWrapper
struct ceres_solver_summary_s {
  ceres::Solver::Summary summary;
};
typedef struct ceres_solver_summary_s ceres_solver_summary_t;

// Wrapper to bridge C callback to Ceres IterationCallback
// Defined here so it can be used in ceres_solver_options_s
class IterationCallbackWrapper : public ceres::IterationCallback {
 public:
  IterationCallbackWrapper(ceres_iteration_callback_t callback, void* user_data)
      : callback_(callback), user_data_(user_data) {}

  ceres::CallbackReturnType operator()(const ceres::IterationSummary& summary) override {
    // Validate callback pointer (user_data_ may legitimately be NULL)
    if (!callback_) {
      return ceres::SOLVER_TERMINATE_SUCCESSFULLY;
    }
    // Create a temporary solver summary wrapper for the callback
    // Note: This is a simplified version - we only pass basic info
    ceres_solver_summary_t temp_summary;
    // IterationSummary doesn't have termination_type, use default
    temp_summary.summary.termination_type = ceres::TerminationType::NO_CONVERGENCE;
    temp_summary.summary.initial_cost = summary.cost;
    temp_summary.summary.final_cost = summary.cost;
    temp_summary.summary.iterations.clear();
    temp_summary.summary.num_successful_steps = summary.step_is_successful ? 1 : 0;
    temp_summary.summary.num_unsuccessful_steps = summary.step_is_successful ? 0 : 1;

    int result = callback_(user_data_, &temp_summary);
    
    // Convert return value: 0 = continue, non-zero = stop
    return (result == 0) ? ceres::SOLVER_CONTINUE : ceres::SOLVER_TERMINATE_SUCCESSFULLY;
  }

 private:
  ceres_iteration_callback_t callback_;
  void* user_data_;
};

// ============================================================================
// Solver Options
// ============================================================================

struct ceres_solver_options_s {
  ceres::Solver::Options options;
  std::unique_ptr<IterationCallbackWrapper> callback_wrapper;  // Store callback wrapper
};

ceres_solver_options_t* ceres_wrapper_create_solver_options() {
  return new ceres_solver_options_t;
}

void ceres_wrapper_free_solver_options(ceres_solver_options_t* options) {
  delete options;
}

void ceres_wrapper_solver_options_set_linear_solver_type(
    ceres_solver_options_t* options, int linear_solver_type) {
  if (!options) return;
  options->options.linear_solver_type =
      static_cast<ceres::LinearSolverType>(linear_solver_type);
}

int ceres_wrapper_solver_options_get_linear_solver_type(
    const ceres_solver_options_t* options) {
  if (!options) return 0;
  return static_cast<int>(options->options.linear_solver_type);
}

void ceres_wrapper_solver_options_set_minimizer_type(
    ceres_solver_options_t* options, int minimizer_type) {
  if (!options) return;
  options->options.minimizer_type =
      static_cast<ceres::MinimizerType>(minimizer_type);
}

int ceres_wrapper_solver_options_get_minimizer_type(
    const ceres_solver_options_t* options) {
  if (!options) return 0;
  return static_cast<int>(options->options.minimizer_type);
}

void ceres_wrapper_solver_options_set_max_num_iterations(
    ceres_solver_options_t* options, int max_num_iterations) {
  if (!options) return;
  options->options.max_num_iterations = max_num_iterations;
}

int ceres_wrapper_solver_options_get_max_num_iterations(
    const ceres_solver_options_t* options) {
  if (!options) return 0;
  return options->options.max_num_iterations;
}

void ceres_wrapper_solver_options_set_num_threads(
    ceres_solver_options_t* options, int num_threads) {
  if (!options) return;
  options->options.num_threads = num_threads;
}

int ceres_wrapper_solver_options_get_num_threads(
    const ceres_solver_options_t* options) {
  if (!options) return 0;
  return options->options.num_threads;
}

void ceres_wrapper_solver_options_set_function_tolerance(
    ceres_solver_options_t* options, double function_tolerance) {
  if (!options) return;
  options->options.function_tolerance = function_tolerance;
}

double ceres_wrapper_solver_options_get_function_tolerance(
    const ceres_solver_options_t* options) {
  if (!options) return 0.0;
  return options->options.function_tolerance;
}

void ceres_wrapper_solver_options_set_gradient_tolerance(
    ceres_solver_options_t* options, double gradient_tolerance) {
  if (!options) return;
  options->options.gradient_tolerance = gradient_tolerance;
}

double ceres_wrapper_solver_options_get_gradient_tolerance(
    const ceres_solver_options_t* options) {
  if (!options) return 0.0;
  return options->options.gradient_tolerance;
}

void ceres_wrapper_solver_options_set_parameter_tolerance(
    ceres_solver_options_t* options, double parameter_tolerance) {
  if (!options) return;
  options->options.parameter_tolerance = parameter_tolerance;
}

double ceres_wrapper_solver_options_get_parameter_tolerance(
    const ceres_solver_options_t* options) {
  if (!options) return 0.0;
  return options->options.parameter_tolerance;
}

void ceres_wrapper_solver_options_set_initial_trust_region_radius(
    ceres_solver_options_t* options, double radius) {
  if (!options) return;
  options->options.initial_trust_region_radius = radius;
}

double ceres_wrapper_solver_options_get_initial_trust_region_radius(
    const ceres_solver_options_t* options) {
  if (!options) return 0.0;
  return options->options.initial_trust_region_radius;
}

void ceres_wrapper_solver_options_set_max_trust_region_radius(
    ceres_solver_options_t* options, double radius) {
  if (!options) return;
  options->options.max_trust_region_radius = radius;
}

double ceres_wrapper_solver_options_get_max_trust_region_radius(
    const ceres_solver_options_t* options) {
  if (!options) return 0.0;
  return options->options.max_trust_region_radius;
}

void ceres_wrapper_solver_options_set_min_trust_region_radius(
    ceres_solver_options_t* options, double radius) {
  if (!options) return;
  options->options.min_trust_region_radius = radius;
}

double ceres_wrapper_solver_options_get_min_trust_region_radius(
    const ceres_solver_options_t* options) {
  if (!options) return 0.0;
  return options->options.min_trust_region_radius;
}

void ceres_wrapper_solver_options_set_preconditioner_type(
    ceres_solver_options_t* options, int preconditioner_type) {
  if (!options) return;
  options->options.preconditioner_type =
      static_cast<ceres::PreconditionerType>(preconditioner_type);
}

int ceres_wrapper_solver_options_get_preconditioner_type(
    const ceres_solver_options_t* options) {
  if (!options) return 0;
  return static_cast<int>(options->options.preconditioner_type);
}

void ceres_wrapper_solver_options_set_trust_region_strategy_type(
    ceres_solver_options_t* options, int trust_region_strategy_type) {
  if (!options) return;
  options->options.trust_region_strategy_type =
      static_cast<ceres::TrustRegionStrategyType>(trust_region_strategy_type);
}

int ceres_wrapper_solver_options_get_trust_region_strategy_type(
    const ceres_solver_options_t* options) {
  if (!options) return 0;
  return static_cast<int>(options->options.trust_region_strategy_type);
}

void ceres_wrapper_solver_options_set_dogleg_type(
    ceres_solver_options_t* options, int dogleg_type) {
  if (!options) return;
  options->options.dogleg_type =
      static_cast<ceres::DoglegType>(dogleg_type);
}

int ceres_wrapper_solver_options_get_dogleg_type(
    const ceres_solver_options_t* options) {
  if (!options) return 0;
  return static_cast<int>(options->options.dogleg_type);
}

void ceres_wrapper_solver_options_set_use_nonmonotonic_steps(
    ceres_solver_options_t* options, int use_nonmonotonic_steps) {
  if (!options) return;
  options->options.use_nonmonotonic_steps = (use_nonmonotonic_steps != 0);
}

int ceres_wrapper_solver_options_get_use_nonmonotonic_steps(
    const ceres_solver_options_t* options) {
  if (!options) return 0;
  return options->options.use_nonmonotonic_steps ? 1 : 0;
}

void ceres_wrapper_solver_options_set_max_consecutive_nonmonotonic_steps(
    ceres_solver_options_t* options, int max_steps) {
  if (!options) return;
  options->options.max_consecutive_nonmonotonic_steps = max_steps;
}

int ceres_wrapper_solver_options_get_max_consecutive_nonmonotonic_steps(
    const ceres_solver_options_t* options) {
  if (!options) return 0;
  return options->options.max_consecutive_nonmonotonic_steps;
}

void ceres_wrapper_solver_options_set_max_num_consecutive_invalid_steps(
    ceres_solver_options_t* options, int max_steps) {
  if (!options) return;
  options->options.max_num_consecutive_invalid_steps = max_steps;
}

int ceres_wrapper_solver_options_get_max_num_consecutive_invalid_steps(
    const ceres_solver_options_t* options) {
  if (!options) return 0;
  return options->options.max_num_consecutive_invalid_steps;
}

void ceres_wrapper_solver_options_set_min_relative_decrease(
    ceres_solver_options_t* options, double min_relative_decrease) {
  if (!options) return;
  options->options.min_relative_decrease = min_relative_decrease;
}

double ceres_wrapper_solver_options_get_min_relative_decrease(
    const ceres_solver_options_t* options) {
  if (!options) return 0.0;
  return options->options.min_relative_decrease;
}

void ceres_wrapper_solver_options_set_logging_type(
    ceres_solver_options_t* options, int logging_type) {
  if (!options) return;
  options->options.logging_type =
      static_cast<ceres::LoggingType>(logging_type);
}

int ceres_wrapper_solver_options_get_logging_type(
    const ceres_solver_options_t* options) {
  if (!options) return 0;
  return static_cast<int>(options->options.logging_type);
}

void ceres_wrapper_solver_options_set_minimizer_progress_to_stdout(
    ceres_solver_options_t* options, int minimizer_progress_to_stdout) {
  if (!options) return;
  options->options.minimizer_progress_to_stdout = (minimizer_progress_to_stdout != 0);
}

int ceres_wrapper_solver_options_get_minimizer_progress_to_stdout(
    const ceres_solver_options_t* options) {
  if (!options) return 0;
  return options->options.minimizer_progress_to_stdout ? 1 : 0;
}

int ceres_wrapper_solver_options_is_valid(
    const ceres_solver_options_t* options, char* error_message, int error_message_size) {
  if (!options) return 0;
  std::string error;
  bool valid = options->options.IsValid(&error);
  if (!valid && error_message && error_message_size > 0) {
    int copy_size = std::min(static_cast<int>(error.size()), error_message_size - 1);
    std::strncpy(error_message, error.c_str(), copy_size);
    error_message[copy_size] = '\0';
  }
  return valid ? 1 : 0;
}

// ============================================================================
// Solver Summary
// ============================================================================

// Note: ceres_solver_summary_s is defined earlier in the file

ceres_solver_summary_t* ceres_wrapper_create_solver_summary() {
  return new ceres_solver_summary_t;
}

void ceres_wrapper_free_solver_summary(ceres_solver_summary_t* summary) {
  delete summary;
}

int ceres_wrapper_solver_summary_get_termination_type(
    const ceres_solver_summary_t* summary) {
  if (!summary) return 0;
  return static_cast<int>(summary->summary.termination_type);
}

void ceres_wrapper_solver_summary_get_message(
    const ceres_solver_summary_t* summary, char* message, int message_size) {
  if (!summary) return;
  if (message && message_size > 0) {
    const std::string& msg = summary->summary.message;
    int copy_size = std::min(static_cast<int>(msg.size()), message_size - 1);
    std::strncpy(message, msg.c_str(), copy_size);
    message[copy_size] = '\0';
  }
}

double ceres_wrapper_solver_summary_get_initial_cost(
    const ceres_solver_summary_t* summary) {
  if (!summary) return 0.0;
  return summary->summary.initial_cost;
}

double ceres_wrapper_solver_summary_get_final_cost(
    const ceres_solver_summary_t* summary) {
  if (!summary) return 0.0;
  return summary->summary.final_cost;
}

int ceres_wrapper_solver_summary_get_iterations(
    const ceres_solver_summary_t* summary) {
  if (!summary) return 0;
  return summary->summary.iterations.size();
}

int ceres_wrapper_solver_summary_get_num_successful_steps(
    const ceres_solver_summary_t* summary) {
  if (!summary) return 0;
  return summary->summary.num_successful_steps;
}

int ceres_wrapper_solver_summary_get_num_unsuccessful_steps(
    const ceres_solver_summary_t* summary) {
  if (!summary) return 0;
  return summary->summary.num_unsuccessful_steps;
}

int ceres_wrapper_solver_summary_get_num_inner_iteration_steps(
    const ceres_solver_summary_t* summary) {
  if (!summary) return 0;
  return summary->summary.num_inner_iteration_steps;
}

double ceres_wrapper_solver_summary_get_total_time_in_seconds(
    const ceres_solver_summary_t* summary) {
  if (!summary) return 0.0;
  return summary->summary.total_time_in_seconds;
}

double ceres_wrapper_solver_summary_get_preprocessor_time_in_seconds(
    const ceres_solver_summary_t* summary) {
  if (!summary) return 0.0;
  return summary->summary.preprocessor_time_in_seconds;
}

double ceres_wrapper_solver_summary_get_minimizer_time_in_seconds(
    const ceres_solver_summary_t* summary) {
  if (!summary) return 0.0;
  return summary->summary.minimizer_time_in_seconds;
}

double ceres_wrapper_solver_summary_get_postprocessor_time_in_seconds(
    const ceres_solver_summary_t* summary) {
  if (!summary) return 0.0;
  return summary->summary.postprocessor_time_in_seconds;
}

double ceres_wrapper_solver_summary_get_linear_solver_time_in_seconds(
    const ceres_solver_summary_t* summary) {
  if (!summary) return 0.0;
  return summary->summary.linear_solver_time_in_seconds;
}

void ceres_wrapper_solver_summary_get_full_report(
    const ceres_solver_summary_t* summary, char* report, int report_size) {
  if (!summary) return;
  if (report && report_size > 0) {
    std::string full_report = summary->summary.FullReport();
    int copy_size = std::min(static_cast<int>(full_report.size()), report_size - 1);
    std::strncpy(report, full_report.c_str(), copy_size);
    report[copy_size] = '\0';
  }
}

// ============================================================================
// Solve with Options and Summary
// ============================================================================

ceres_wrapper_error_code_t ceres_wrapper_solve(
    ceres_problem_t* problem,
    const ceres_solver_options_t* options,
    ceres_solver_summary_t* summary,
    char* error_message,
    int error_message_size) {
  if (!problem) {
    fprintf(stdout, "E20251230 ceres_wrapper_solve: ERROR - problem is null\n");
    fflush(stdout);
    if (error_message && error_message_size > 0) {
      std::strncpy(error_message, "problem is null", error_message_size - 1);
      error_message[error_message_size - 1] = '\0';
    }
    return CERES_WRAPPER_ERROR_NULL_POINTER;
  }
  if (!options) {
    fprintf(stdout, "E20251230 ceres_wrapper_solve: ERROR - options is null\n");
    fflush(stdout);
    if (error_message && error_message_size > 0) {
      std::strncpy(error_message, "options is null", error_message_size - 1);
      error_message[error_message_size - 1] = '\0';
    }
    return CERES_WRAPPER_ERROR_NULL_POINTER;
  }
  if (!summary) {
    fprintf(stdout, "E20251230 ceres_wrapper_solve: ERROR - summary is null\n");
    fflush(stdout);
    if (error_message && error_message_size > 0) {
      std::strncpy(error_message, "summary is null", error_message_size - 1);
      error_message[error_message_size - 1] = '\0';
    }
    return CERES_WRAPPER_ERROR_NULL_POINTER;
  }
  
  try {
    auto* ceres_problem = reinterpret_cast<ceres::Problem*>(problem);
    
    // Forward call to Ceres - with detailed logging
    ceres::Solve(options->options, ceres_problem, &summary->summary);
    
    return CERES_WRAPPER_SUCCESS;
  } catch (const std::exception& e) {
    fprintf(stdout, "E20251230 ceres_wrapper_solve: std::exception caught - what()=%s\n", e.what());
    fflush(stdout);
    if (error_message && error_message_size > 0) {
      std::strncpy(error_message, e.what(), error_message_size - 1);
      error_message[error_message_size - 1] = '\0';
    }
    return CERES_WRAPPER_ERROR_EXCEPTION;
  } catch (...) {
    fprintf(stdout, "E20251230 ceres_wrapper_solve: Unknown exception caught\n");
    fflush(stdout);
    if (error_message && error_message_size > 0) {
      std::strncpy(error_message, "Unknown exception during solve", error_message_size - 1);
      error_message[error_message_size - 1] = '\0';
    }
    return CERES_WRAPPER_ERROR_EXCEPTION;
  }
}

// ============================================================================
// Problem Creation (Official C API)
// ============================================================================

ceres_problem_t* ceres_create_problem() {
  try {
    return reinterpret_cast<ceres_problem_t*>(new ceres::Problem());
  } catch (...) {
    return nullptr;
  }
}

void ceres_free_problem(ceres_problem_t* problem) {
  if (problem) {
    delete reinterpret_cast<ceres::Problem*>(problem);
  }
}

// ============================================================================
// Parameter Blocks Management
// ============================================================================

ceres_wrapper_error_code_t ceres_wrapper_problem_add_parameter_block(
    ceres_problem_t* problem,
    double* parameters,
    int size,
    char* error_message,
    int error_message_size) {
  if (!problem) {
    if (error_message && error_message_size > 0) {
      std::strncpy(error_message, "problem is null", error_message_size - 1);
      error_message[error_message_size - 1] = '\0';
    }
    return CERES_WRAPPER_ERROR_NULL_POINTER;
  }
  if (!parameters) {
    if (error_message && error_message_size > 0) {
      std::strncpy(error_message, "parameters is null", error_message_size - 1);
      error_message[error_message_size - 1] = '\0';
    }
    return CERES_WRAPPER_ERROR_NULL_POINTER;
  }
  if (size <= 0) {
    if (error_message && error_message_size > 0) {
      std::strncpy(error_message, "size must be positive", error_message_size - 1);
      error_message[error_message_size - 1] = '\0';
    }
    return CERES_WRAPPER_ERROR_INVALID_PARAMETER;
  }
  
  try {
    auto* ceres_problem = reinterpret_cast<ceres::Problem*>(problem);
    ceres_problem->AddParameterBlock(parameters, size);
    return CERES_WRAPPER_SUCCESS;
  } catch (const std::exception& e) {
    if (error_message && error_message_size > 0) {
      std::strncpy(error_message, e.what(), error_message_size - 1);
      error_message[error_message_size - 1] = '\0';
    }
    return CERES_WRAPPER_ERROR_EXCEPTION;
  } catch (...) {
    if (error_message && error_message_size > 0) {
      std::strncpy(error_message, "Unknown exception", error_message_size - 1);
      error_message[error_message_size - 1] = '\0';
    }
    return CERES_WRAPPER_ERROR_EXCEPTION;
  }
}

void ceres_wrapper_problem_set_parameter_block_constant(
    ceres_problem_t* problem,
    double* parameters) {
  if (!problem || !parameters) return;
  try {
    auto* ceres_problem = reinterpret_cast<ceres::Problem*>(problem);
    ceres_problem->SetParameterBlockConstant(parameters);
  } catch (...) {
    // Prevent C++ exception from crossing extern "C" boundary
  }
}

void ceres_wrapper_problem_set_parameter_block_variable(
    ceres_problem_t* problem,
    double* parameters) {
  if (!problem || !parameters) return;
  try {
    auto* ceres_problem = reinterpret_cast<ceres::Problem*>(problem);
    ceres_problem->SetParameterBlockVariable(parameters);
  } catch (...) {
    // Prevent C++ exception from crossing extern "C" boundary
  }
}

ceres_wrapper_error_code_t ceres_wrapper_problem_remove_parameter_block(
    ceres_problem_t* problem,
    double* parameters,
    char* error_message,
    int error_message_size) {
  if (!problem) {
    if (error_message && error_message_size > 0) {
      std::strncpy(error_message, "problem is null", error_message_size - 1);
      error_message[error_message_size - 1] = '\0';
    }
    return CERES_WRAPPER_ERROR_NULL_POINTER;
  }
  if (!parameters) {
    if (error_message && error_message_size > 0) {
      std::strncpy(error_message, "parameters is null", error_message_size - 1);
      error_message[error_message_size - 1] = '\0';
    }
    return CERES_WRAPPER_ERROR_NULL_POINTER;
  }
  
  try {
    auto* ceres_problem = reinterpret_cast<ceres::Problem*>(problem);
    ceres_problem->RemoveParameterBlock(parameters);
    return CERES_WRAPPER_SUCCESS;
  } catch (const std::exception& e) {
    if (error_message && error_message_size > 0) {
      std::strncpy(error_message, e.what(), error_message_size - 1);
      error_message[error_message_size - 1] = '\0';
    }
    return CERES_WRAPPER_ERROR_EXCEPTION;
  } catch (...) {
    if (error_message && error_message_size > 0) {
      std::strncpy(error_message, "Unknown exception", error_message_size - 1);
      error_message[error_message_size - 1] = '\0';
    }
    return CERES_WRAPPER_ERROR_EXCEPTION;
  }
}

ceres_wrapper_error_code_t ceres_wrapper_problem_remove_residual_block(
    ceres_problem_t* problem,
    ceres_residual_block_id_t* residual_block_id,
    char* error_message,
    int error_message_size) {
  if (!problem) {
    if (error_message && error_message_size > 0) {
      std::strncpy(error_message, "problem is null", error_message_size - 1);
      error_message[error_message_size - 1] = '\0';
    }
    return CERES_WRAPPER_ERROR_NULL_POINTER;
  }
  if (!residual_block_id) {
    if (error_message && error_message_size > 0) {
      std::strncpy(error_message, "residual_block_id is null", error_message_size - 1);
      error_message[error_message_size - 1] = '\0';
    }
    return CERES_WRAPPER_ERROR_NULL_POINTER;
  }
  
  try {
    auto* ceres_problem = reinterpret_cast<ceres::Problem*>(problem);
    auto* block_id = reinterpret_cast<ceres::ResidualBlockId>(residual_block_id);
    ceres_problem->RemoveResidualBlock(block_id);
    return CERES_WRAPPER_SUCCESS;
  } catch (const std::exception& e) {
    if (error_message && error_message_size > 0) {
      std::strncpy(error_message, e.what(), error_message_size - 1);
      error_message[error_message_size - 1] = '\0';
    }
    return CERES_WRAPPER_ERROR_EXCEPTION;
  } catch (...) {
    if (error_message && error_message_size > 0) {
      std::strncpy(error_message, "Unknown exception", error_message_size - 1);
      error_message[error_message_size - 1] = '\0';
    }
    return CERES_WRAPPER_ERROR_EXCEPTION;
  }
}

// ============================================================================
// Loss Functions (defined early for use in AddResidualBlock)
// ============================================================================

struct ceres_wrapper_loss_function_s {
  std::unique_ptr<ceres::LossFunction> loss;
};

ceres_wrapper_error_code_t ceres_wrapper_problem_add_residual_block(
    ceres_problem_t* problem,
    void* cost_function,
    void* loss_function,
    double** parameter_blocks,
    int num_parameter_blocks,
    ceres_residual_block_id_t** residual_block_id,
    char* error_message,
    int error_message_size) {
  if (!problem) {
    if (error_message && error_message_size > 0) {
      std::strncpy(error_message, "problem is null", error_message_size - 1);
      error_message[error_message_size - 1] = '\0';
    }
    return CERES_WRAPPER_ERROR_NULL_POINTER;
  }
  if (!cost_function) {
    if (error_message && error_message_size > 0) {
      std::strncpy(error_message, "cost_function is null", error_message_size - 1);
      error_message[error_message_size - 1] = '\0';
    }
    return CERES_WRAPPER_ERROR_NULL_POINTER;
  }
  if (!parameter_blocks) {
    if (error_message && error_message_size > 0) {
      std::strncpy(error_message, "parameter_blocks is null", error_message_size - 1);
      error_message[error_message_size - 1] = '\0';
    }
    return CERES_WRAPPER_ERROR_NULL_POINTER;
  }
  if (num_parameter_blocks <= 0) {
    if (error_message && error_message_size > 0) {
      std::strncpy(error_message, "num_parameter_blocks must be positive", error_message_size - 1);
      error_message[error_message_size - 1] = '\0';
    }
    return CERES_WRAPPER_ERROR_INVALID_PARAMETER;
  }
  
  try {
    auto* ceres_problem = reinterpret_cast<ceres::Problem*>(problem);
    auto* ceres_cost = reinterpret_cast<ceres::CostFunction*>(cost_function);
    
    // Handle loss function
    ceres::LossFunction* ceres_loss = nullptr;
    if (loss_function) {
      auto* loss_wrapper = reinterpret_cast<ceres_wrapper_loss_function_t*>(loss_function);
      ceres_loss = loss_wrapper->loss.get();  // Don't release yet
    }

    // Add residual block
    // Problem takes ownership of cost_function and loss_function by default
    auto* block_id = ceres_problem->AddResidualBlock(
        ceres_cost, ceres_loss, parameter_blocks, num_parameter_blocks);

    // Only release ownership AFTER successful AddResidualBlock
    if (loss_function) {
      auto* loss_wrapper = reinterpret_cast<ceres_wrapper_loss_function_t*>(loss_function);
      loss_wrapper->loss.release();  // Now safe to release
    }

    // Set output parameter if provided
    if (residual_block_id) {
      *residual_block_id = reinterpret_cast<ceres_residual_block_id_t*>(block_id);
    }
    
    return CERES_WRAPPER_SUCCESS;
  } catch (const std::exception& e) {
    if (error_message && error_message_size > 0) {
      std::strncpy(error_message, e.what(), error_message_size - 1);
      error_message[error_message_size - 1] = '\0';
    }
    return CERES_WRAPPER_ERROR_EXCEPTION;
  } catch (...) {
    if (error_message && error_message_size > 0) {
      std::strncpy(error_message, "Unknown exception", error_message_size - 1);
      error_message[error_message_size - 1] = '\0';
    }
    return CERES_WRAPPER_ERROR_EXCEPTION;
  }
}

int ceres_wrapper_problem_num_parameter_blocks(
    const ceres_problem_t* problem) {
  if (!problem) return 0;
  const auto* ceres_problem = reinterpret_cast<const ceres::Problem*>(problem);
  return ceres_problem->NumParameterBlocks();
}

int ceres_wrapper_problem_num_residual_blocks(
    const ceres_problem_t* problem) {
  if (!problem) return 0;
  const auto* ceres_problem = reinterpret_cast<const ceres::Problem*>(problem);
  return ceres_problem->NumResidualBlocks();
}

int ceres_wrapper_problem_num_parameters(
    const ceres_problem_t* problem) {
  if (!problem) return 0;
  const auto* ceres_problem = reinterpret_cast<const ceres::Problem*>(problem);
  return ceres_problem->NumParameters();
}

int ceres_wrapper_problem_num_residuals(
    const ceres_problem_t* problem) {
  if (!problem) return 0;
  const auto* ceres_problem = reinterpret_cast<const ceres::Problem*>(problem);
  return ceres_problem->NumResiduals();
}

// ============================================================================
// Loss Functions (struct already defined above for AddResidualBlock)
// ============================================================================

ceres_wrapper_loss_function_t* ceres_wrapper_create_huber_loss(double a) {
  try {
    auto* loss = new ceres_wrapper_loss_function_s;
    loss->loss = std::make_unique<ceres::HuberLoss>(a);
    return loss;
  } catch (...) {
    return nullptr;
  }
}

ceres_wrapper_loss_function_t* ceres_wrapper_create_trivial_loss() {
  try {
    auto* loss = new ceres_wrapper_loss_function_s;
    loss->loss = std::make_unique<ceres::TrivialLoss>();
    return loss;
  } catch (...) {
    return nullptr;
  }
}

ceres_wrapper_loss_function_t* ceres_wrapper_create_cauchy_loss(double a) {
  try {
    auto* loss = new ceres_wrapper_loss_function_s;
    loss->loss = std::make_unique<ceres::CauchyLoss>(a);
    return loss;
  } catch (...) {
    return nullptr;
  }
}

ceres_wrapper_loss_function_t* ceres_wrapper_create_softl1_loss(double a) {
  try {
    auto* loss = new ceres_wrapper_loss_function_s;
    loss->loss = std::make_unique<ceres::SoftLOneLoss>(a);
    return loss;
  } catch (...) {
    return nullptr;
  }
}

ceres_wrapper_loss_function_t* ceres_wrapper_create_arctan_loss(double a) {
  try {
    auto* loss = new ceres_wrapper_loss_function_s;
    loss->loss = std::make_unique<ceres::ArctanLoss>(a);
    return loss;
  } catch (...) {
    return nullptr;
  }
}

ceres_wrapper_loss_function_t* ceres_wrapper_create_tolerant_loss(double a, double b) {
  try {
    auto* loss = new ceres_wrapper_loss_function_s;
    loss->loss = std::make_unique<ceres::TolerantLoss>(a, b);
    return loss;
  } catch (...) {
    return nullptr;
  }
}

void ceres_wrapper_free_loss_function(ceres_wrapper_loss_function_t* loss) {
  delete loss;
}

ceres_wrapper_loss_function_t* ceres_wrapper_create_composed_loss(
    ceres_wrapper_loss_function_t* f,
    int ownership_f,
    ceres_wrapper_loss_function_t* g,
    int ownership_g) {
  if (!f || !g) {
    return nullptr;
  }
  
  try {
    auto* loss = new ceres_wrapper_loss_function_s;

    ceres::LossFunction* ceres_f = f->loss.release();
    ceres::LossFunction* ceres_g = g->loss.release();

    ceres::Ownership ownership_f_enum = ownership_f ? ceres::TAKE_OWNERSHIP : ceres::DO_NOT_TAKE_OWNERSHIP;
    ceres::Ownership ownership_g_enum = ownership_g ? ceres::TAKE_OWNERSHIP : ceres::DO_NOT_TAKE_OWNERSHIP;

    try {
      loss->loss = std::make_unique<ceres::ComposedLoss>(
          ceres_f, ownership_f_enum, ceres_g, ownership_g_enum);
    } catch (...) {
      // If ownership was TAKE_OWNERSHIP, ComposedLoss ctor would have taken ownership
      // But if it threw before, we need to clean up
      if (ownership_f_enum == ceres::DO_NOT_TAKE_OWNERSHIP) delete ceres_f;
      if (ownership_g_enum == ceres::DO_NOT_TAKE_OWNERSHIP) delete ceres_g;
      delete loss;
      delete f;
      delete g;
      return nullptr;
    }

    // Free the wrapper structures (we always release the unique_ptr above)
    delete f;
    delete g;

    return loss;
  } catch (...) {
    return nullptr;
  }
}

ceres_wrapper_loss_function_t* ceres_wrapper_create_scaled_loss(
    ceres_wrapper_loss_function_t* rho,
    double a,
    int ownership) {
  try {
    auto* loss = new ceres_wrapper_loss_function_s;
    
    // Extract loss function from wrapper
    // Note: We release from unique_ptr because ScaledLoss will manage ownership
    ceres::LossFunction* ceres_rho = nullptr;
    if (rho) {
      ceres_rho = rho->loss.release();
    }
    
    // Create ScaledLoss
    // If ownership = 1, ScaledLoss takes ownership (will delete when done)
    // If ownership = 0, ScaledLoss does not take ownership (caller manages)
    ceres::Ownership ownership_enum = ownership ? ceres::TAKE_OWNERSHIP : ceres::DO_NOT_TAKE_OWNERSHIP;
    loss->loss = std::make_unique<ceres::ScaledLoss>(ceres_rho, a, ownership_enum);
    
    // Free the wrapper structure (we always release the unique_ptr above if rho was provided)
    if (rho) {
      delete rho;
    }
    
    return loss;
  } catch (...) {
    return nullptr;
  }
}

// ============================================================================
// Manifolds
// ============================================================================

// Custom deleter type for manifolds (allows non-owning pointers)
using ManifoldDeleter = std::function<void(ceres::Manifold*)>;
using ManifoldPtr = std::unique_ptr<ceres::Manifold, ManifoldDeleter>;

struct ceres_manifold_s {
  ManifoldPtr manifold;
};

ceres_manifold_t* ceres_wrapper_create_quaternion_manifold() {
  auto* m = new ceres_manifold_t;
  // Create with default deleter (owning)
  auto ptr = std::make_unique<ceres::QuaternionManifold>();
  m->manifold = ManifoldPtr(ptr.release(), [](ceres::Manifold* p) { delete p; });
  return m;
}

ceres_manifold_t* ceres_wrapper_create_sphere_manifold(int dimension) {
  auto* m = new ceres_manifold_t;
  // Create with default deleter (owning)
  auto ptr = std::make_unique<ceres::SphereManifold<ceres::DYNAMIC>>(dimension);
  m->manifold = ManifoldPtr(ptr.release(), [](ceres::Manifold* p) { delete p; });
  return m;
}

ceres_manifold_t* ceres_wrapper_create_line_manifold(int dimension) {
  auto* m = new ceres_manifold_t;
  // LineManifold is a template class, use DYNAMIC for runtime dimension
  // Create with default deleter (owning)
  auto ptr = std::make_unique<ceres::LineManifold<ceres::DYNAMIC>>(dimension);
  m->manifold = ManifoldPtr(ptr.release(), [](ceres::Manifold* p) { delete p; });
  return m;
}

ceres_manifold_t* ceres_wrapper_create_euclidean_manifold(int dimension) {
  auto* m = new ceres_manifold_t;
  // EuclideanManifold is a template class, use DYNAMIC for runtime dimension
  auto ptr = std::make_unique<ceres::EuclideanManifold<ceres::DYNAMIC>>(dimension);
  m->manifold = ManifoldPtr(ptr.release(), [](ceres::Manifold* p) { delete p; });
  return m;
}

ceres_manifold_t* ceres_wrapper_create_subset_manifold(
    const int* constant_subset,
    int constant_subset_size,
    int ambient_size) {
  if (!constant_subset || constant_subset_size <= 0 || ambient_size <= 0) {
    return nullptr;
  }
  
  auto* m = new ceres_manifold_t;
  // Convert C array to std::vector
  std::vector<int> constant_indices(constant_subset, constant_subset + constant_subset_size);
  // SubsetManifold constructor: SubsetManifold(int size, const std::vector<int>& constant_parameters)
  auto ptr = std::make_unique<ceres::SubsetManifold>(
      ambient_size, constant_indices);
  m->manifold = ManifoldPtr(ptr.release(), [](ceres::Manifold* p) { delete p; });
  return m;
}

void ceres_wrapper_free_manifold(ceres_manifold_t* manifold) {
  delete manifold;
}

void ceres_wrapper_problem_set_manifold(
    ceres_problem_t* problem,
    double* parameters,
    ceres_manifold_t* manifold) {
  auto* ceres_problem = reinterpret_cast<ceres::Problem*>(problem);
  // Problem takes ownership of the manifold (TAKE_OWNERSHIP by default)
  // We need to release the unique_ptr before passing to Problem
  ceres::Manifold* manifold_ptr = manifold->manifold.release();
  ceres_problem->SetManifold(parameters, manifold_ptr);
  // Note: Problem now owns the manifold, it will be freed when problem is destroyed
  // We can still free the wrapper struct, but the manifold itself is owned by Problem
}

int ceres_wrapper_manifold_ambient_size(const ceres_manifold_t* manifold) {
  if (!manifold || !manifold->manifold) return 0;
  return manifold->manifold->AmbientSize();
}

int ceres_wrapper_manifold_tangent_size(const ceres_manifold_t* manifold) {
  if (!manifold || !manifold->manifold) return 0;
  return manifold->manifold->TangentSize();
}

// ============================================================================
// AutoDiff Manifold
// ============================================================================

// Wrapper class to bridge C callbacks to Ceres AutoDiffManifold
// Since AutoDiffManifold is a template class, we create a custom Manifold
// that uses callbacks and numeric differentiation for Jacobians
class AutoDiffManifoldWrapper : public ceres::Manifold {
 public:
  AutoDiffManifoldWrapper(
      int ambient_size,
      int tangent_size,
      ceres_autodiff_manifold_plus_t plus_callback,
      ceres_autodiff_manifold_minus_t minus_callback,
      void* user_data)
      : ambient_size_(ambient_size),
        tangent_size_(tangent_size),
        plus_callback_(plus_callback),
        minus_callback_(minus_callback),
        user_data_(user_data) {
    if (ambient_size <= 0 || tangent_size <= 0) {
      throw std::invalid_argument("Ambient size and tangent size must be positive");
    }
  }

  int AmbientSize() const override { return ambient_size_; }
  int TangentSize() const override { return tangent_size_; }

  bool Plus(const double* x,
            const double* delta,
            double* x_plus_delta) const override {
    if (!plus_callback_) {
      return false;
    }
    // user_data_ may legitimately be NULL if callback doesn't need state
    int result = plus_callback_(user_data_, x, delta, x_plus_delta);
    return (result != 0);
  }

  bool PlusJacobian(const double* x, double* jacobian) const override {
    // Use numeric differentiation for PlusJacobian
    const double kEpsilon = 1e-8;
    std::vector<double> zero_delta(tangent_size_, 0.0);
    std::vector<double> x_plus_delta(ambient_size_);
    std::vector<double> perturbed_delta(tangent_size_);
    std::vector<double> perturbed_x_plus_delta(ambient_size_);

    // Compute Plus(x, 0) as baseline
    if (!Plus(x, zero_delta.data(), x_plus_delta.data())) {
      return false;
    }

    // Compute Jacobian column by column
    for (int j = 0; j < tangent_size_; ++j) {
      // Perturb delta[j]
      std::copy(zero_delta.data(), zero_delta.data() + tangent_size_, perturbed_delta.data());
      perturbed_delta[j] = kEpsilon;

      // Compute Plus(x, perturbed_delta)
      if (!Plus(x, perturbed_delta.data(), perturbed_x_plus_delta.data())) {
        return false;
      }

      // Compute finite difference: (Plus(x, perturbed_delta) - Plus(x, 0)) / epsilon
      for (int i = 0; i < ambient_size_; ++i) {
        jacobian[i * tangent_size_ + j] =
            (perturbed_x_plus_delta[i] - x_plus_delta[i]) / kEpsilon;
      }
    }

    return true;
  }

  bool Minus(const double* y,
             const double* x,
             double* y_minus_x) const override {
    if (!minus_callback_) {
      return false;
    }
    // user_data_ may legitimately be NULL if callback doesn't need state
    int result = minus_callback_(user_data_, y, x, y_minus_x);
    return (result != 0);
  }

  bool MinusJacobian(const double* x, double* jacobian) const override {
    // Use numeric differentiation for MinusJacobian
    // MinusJacobian is D_1 Minus(x, x), so we compute derivative w.r.t. first argument
    const double kEpsilon = 1e-8;
    std::vector<double> y_minus_x(tangent_size_);
    std::vector<double> perturbed_y(ambient_size_);
    std::vector<double> perturbed_y_minus_x(tangent_size_);

    // Compute Minus(x, x) as baseline (should be zero)
    if (!Minus(x, x, y_minus_x.data())) {
      return false;
    }

    // Compute Jacobian column by column
    for (int j = 0; j < ambient_size_; ++j) {
      // Perturb y[j] (first argument)
      std::copy(x, x + ambient_size_, perturbed_y.data());
      perturbed_y[j] += kEpsilon;

      // Compute Minus(perturbed_y, x)
      if (!Minus(perturbed_y.data(), x, perturbed_y_minus_x.data())) {
        return false;
      }

      // Compute finite difference: (Minus(perturbed_y, x) - Minus(x, x)) / epsilon
      for (int i = 0; i < tangent_size_; ++i) {
        jacobian[i * ambient_size_ + j] =
            (perturbed_y_minus_x[i] - y_minus_x[i]) / kEpsilon;
      }
    }

    return true;
  }

 private:
  int ambient_size_;
  int tangent_size_;
  ceres_autodiff_manifold_plus_t plus_callback_;
  ceres_autodiff_manifold_minus_t minus_callback_;
  void* user_data_;
};

ceres_manifold_t* ceres_wrapper_create_autodiff_manifold(
    int ambient_size,
    int tangent_size,
    ceres_autodiff_manifold_plus_t plus_callback,
    ceres_autodiff_manifold_minus_t minus_callback,
    void* user_data) {
  if (ambient_size <= 0 || tangent_size <= 0) {
    return nullptr;
  }
  if (!plus_callback || !minus_callback) {
    return nullptr;
  }

  try {
    auto* m = new ceres_manifold_t;
    auto wrapper = std::make_unique<AutoDiffManifoldWrapper>(
        ambient_size, tangent_size, plus_callback, minus_callback, user_data);
    m->manifold = ManifoldPtr(wrapper.release(), [](ceres::Manifold* p) { delete p; });
    return m;
  } catch (...) {
    return nullptr;
  }
}

void ceres_wrapper_free_autodiff_manifold(ceres_manifold_t* manifold) {
  // Same as free_manifold - just delete the wrapper struct
  // The manifold itself might be owned by Problem
  delete manifold;
}

// ============================================================================
// BiCubic Interpolator
// ============================================================================

// Adapter for 2D grid data to work with Ceres BiCubicInterpolator
class GridArrayAdapter {
 public:
  enum { DATA_DIMENSION = 1 };

  GridArrayAdapter(const double* data, int rows, int cols)
      : data_(data), rows_(rows), cols_(cols) {}

  void GetValue(const int row, const int column, double* const value) const {
    if (row < 0 || column < 0 || row >= rows_ || column >= cols_) {
      *value = 0.0;  // Out of bounds - return default value
    } else {
      *value = data_[row * cols_ + column];
    }
  }

  int NumRows() const { return rows_; }
  int NumCols() const { return cols_; }

 private:
  const double* data_;
  int rows_;
  int cols_;
};

struct ceres_bicubic_interpolator_s {
  std::unique_ptr<ceres::BiCubicInterpolator<GridArrayAdapter>> interpolator;
  std::unique_ptr<GridArrayAdapter> adapter;
  std::vector<double> data;  // Keep data alive
};

ceres_bicubic_interpolator_t* ceres_wrapper_create_bicubic_interpolator(
    const double* data,
    int rows,
    int cols) {
  if (!data || rows <= 0 || cols <= 0) {
    return nullptr;
  }
  auto* interp = new ceres_bicubic_interpolator_t;
  
  // Copy data to keep it alive
  interp->data.assign(data, data + rows * cols);
  
  // Create adapter with copied data
  interp->adapter = std::make_unique<GridArrayAdapter>(
      interp->data.data(), rows, cols);
  
  // Create interpolator with adapter
  interp->interpolator = std::make_unique<ceres::BiCubicInterpolator<GridArrayAdapter>>(
      *interp->adapter);
  
  return interp;
}

void ceres_wrapper_free_bicubic_interpolator(
    ceres_bicubic_interpolator_t* interpolator) {
  delete interpolator;
}

void ceres_wrapper_bicubic_interpolator_evaluate(
    const ceres_bicubic_interpolator_t* interpolator,
    double x,
    double y,
    double* value,
    double* gradient_x,
    double* gradient_y) {
  double val = 0.0;
  double grad_r = 0.0;  // gradient with respect to row
  double grad_c = 0.0;  // gradient with respect to column
  
  // Evaluate: (row, column) = (y, x)
  // Note: Ceres uses (r, c) = (row, column) convention
  interpolator->interpolator->Evaluate(y, x, &val, &grad_r, &grad_c);
  
  if (value) *value = val;
  // Map gradients: grad_r (row) -> grad_y, grad_c (column) -> grad_x
  if (gradient_x) *gradient_x = grad_c;
  if (gradient_y) *gradient_y = grad_r;
}

// ============================================================================
// AutoDiff Cost Function Wrapper
// ============================================================================

// Wrapper class to bridge C callback to Ceres AutoDiffCostFunction
class AutoDiffCostFunctionWrapper : public ceres::CostFunction {
 public:
  AutoDiffCostFunctionWrapper(
      ceres_autodiff_cost_function_callback_t callback,
      void* user_data,
      int num_residuals,
      int num_parameter_blocks,
      const int* parameter_block_sizes)
      : callback_(callback), user_data_(user_data) {
    set_num_residuals(num_residuals);
    for (int i = 0; i < num_parameter_blocks; ++i) {
      mutable_parameter_block_sizes()->push_back(parameter_block_sizes[i]);
    }
  }

  bool Evaluate(double const* const* parameters,
                double* residuals,
                double** jacobians) const override {
    // Validate callback pointer (user_data_ may legitimately be NULL)
    if (!callback_) {
      return false;
    }
    // Call the C callback to get residuals
    int result = callback_(user_data_, parameters, residuals);
    if (result == 0) {
      return false;
    }

    // Basic validation: check for NaN/Inf in residuals
    for (int i = 0; i < num_residuals(); ++i) {
      if (std::isnan(residuals[i]) || std::isinf(residuals[i])) {
        return false;
      }
    }

    // If jacobians are requested, compute them using finite differences
    // Note: True AutoDiff requires template magic, so we use numerical differentiation
    if (jacobians != nullptr) {
      const double kEpsilon = 1e-4;
      std::vector<double> perturbed_residuals(num_residuals());
      int num_blocks = static_cast<int>(parameter_block_sizes().size());

      for (int i = 0; i < num_blocks; ++i) {
        if (jacobians[i] != nullptr) {
          int param_size = parameter_block_sizes()[i];
          std::vector<double> perturbed_params(param_size);

          // Create array of pointers to parameter blocks
          // All blocks point to original parameters except block i which points to perturbed_params
          std::vector<const double*> perturbed_param_ptrs(num_blocks);
          for (int k = 0; k < num_blocks; ++k) {
            perturbed_param_ptrs[k] = parameters[k];
          }

          for (int j = 0; j < param_size; ++j) {
            // Perturb parameter
            std::copy(parameters[i], parameters[i] + param_size, perturbed_params.data());
            perturbed_params[j] += kEpsilon;

            // Update pointer for block i to point to perturbed data
            perturbed_param_ptrs[i] = perturbed_params.data();

            // Evaluate with perturbed parameter (pass array of all parameter block pointers)
            int perturb_result = callback_(user_data_, perturbed_param_ptrs.data(), perturbed_residuals.data());
            if (perturb_result == 0) {
              return false;
            }
            
            // Check for NaN/Inf in perturbed residuals
            for (int k = 0; k < num_residuals(); ++k) {
              if (std::isnan(perturbed_residuals[k]) || std::isinf(perturbed_residuals[k])) {
                return false;
              }
            }
            
            // Compute finite difference
            for (int k = 0; k < num_residuals(); ++k) {
              double diff = (perturbed_residuals[k] - residuals[k]) / kEpsilon;
              if (std::isnan(diff) || std::isinf(diff)) {
                return false;
              }
              jacobians[i][k * param_size + j] = diff;
            }
          }
        }
      }
    }

    return true;
  }

 private:
  ceres_autodiff_cost_function_callback_t callback_;
  void* user_data_;
};

void* ceres_wrapper_create_autodiff_cost_function(
    ceres_autodiff_cost_function_callback_t callback,
    void* user_data,
    int num_residuals,
    int num_parameter_blocks,
    const int* parameter_block_sizes) {
  return new AutoDiffCostFunctionWrapper(
      callback, user_data, num_residuals, num_parameter_blocks, parameter_block_sizes);
}

void ceres_wrapper_free_autodiff_cost_function(void* cost_function) {
  delete reinterpret_cast<AutoDiffCostFunctionWrapper*>(cost_function);
}

// ============================================================================
// Dynamic AutoDiff Cost Function Wrapper
// ============================================================================

// Wrapper class to bridge C callback to Ceres DynamicAutoDiffCostFunction
class DynamicAutoDiffCostFunctionWrapper : public ceres::DynamicCostFunction {
 public:
  DynamicAutoDiffCostFunctionWrapper(
      ceres_autodiff_cost_function_callback_t callback,
      void* user_data,
      int num_residuals,
      int num_parameter_blocks,
      const int* parameter_block_sizes)
      : callback_(callback), user_data_(user_data) {
    set_num_residuals(num_residuals);
    for (int i = 0; i < num_parameter_blocks; ++i) {
      mutable_parameter_block_sizes()->push_back(parameter_block_sizes[i]);
    }
  }

  bool Evaluate(double const* const* parameters,
                double* residuals,
                double** jacobians) const override {
    // Validate callback pointer (user_data_ may legitimately be NULL)
    if (!callback_) {
      return false;
    }
    // Call the C callback to get residuals
    int result = callback_(user_data_, parameters, residuals);
    if (result == 0) {
      return false;
    }

    // Basic validation: check for NaN/Inf in residuals
    for (int i = 0; i < num_residuals(); ++i) {
      if (std::isnan(residuals[i]) || std::isinf(residuals[i])) {
        return false;
      }
    }

    // If jacobians are requested, compute them using finite differences
    if (jacobians != nullptr) {
      const double kEpsilon = 1e-4;
      std::vector<double> perturbed_residuals(num_residuals());
      
      for (int i = 0; i < static_cast<int>(parameter_block_sizes().size()); ++i) {
        if (jacobians[i] != nullptr) {
          int param_size = parameter_block_sizes()[i];
          std::vector<double> perturbed_params(param_size);
          
          // Create array of pointers for all parameter blocks
          std::vector<const double*> param_ptrs(parameter_block_sizes().size());
          for (int k = 0; k < static_cast<int>(parameter_block_sizes().size()); ++k) {
            param_ptrs[k] = parameters[k];
          }
          
          for (int j = 0; j < param_size; ++j) {
            // Perturb parameter
            std::copy(parameters[i], parameters[i] + param_size, perturbed_params.data());
            perturbed_params[j] += kEpsilon;
            param_ptrs[i] = perturbed_params.data();
            
            // Evaluate with perturbed parameter
            int perturb_result = callback_(user_data_, param_ptrs.data(), perturbed_residuals.data());
            if (perturb_result == 0) {
              return false;
            }
            
            // Check for NaN/Inf in perturbed residuals
            for (int k = 0; k < num_residuals(); ++k) {
              if (std::isnan(perturbed_residuals[k]) || std::isinf(perturbed_residuals[k])) {
                return false;
              }
            }
            
            // Compute finite difference
            for (int k = 0; k < num_residuals(); ++k) {
              double diff = (perturbed_residuals[k] - residuals[k]) / kEpsilon;
              if (std::isnan(diff) || std::isinf(diff)) {
                return false;
              }
              jacobians[i][k * param_size + j] = diff;
            }
          }
        }
      }
    }

    return true;
  }

 private:
  ceres_autodiff_cost_function_callback_t callback_;
  void* user_data_;
};

void* ceres_wrapper_create_dynamic_autodiff_cost_function(
    ceres_autodiff_cost_function_callback_t callback,
    void* user_data,
    int num_residuals,
    int num_parameter_blocks,
    const int* parameter_block_sizes) {
  return new DynamicAutoDiffCostFunctionWrapper(
      callback, user_data, num_residuals, num_parameter_blocks, parameter_block_sizes);
}

void ceres_wrapper_free_dynamic_autodiff_cost_function(void* cost_function) {
  delete reinterpret_cast<DynamicAutoDiffCostFunctionWrapper*>(cost_function);
}

// ============================================================================
// Product Manifold
// ============================================================================

// Custom manifold wrapper for runtime ProductManifold
// Since Ceres ProductManifold requires compile-time template parameters,
// we create a custom manifold that wraps multiple manifolds at runtime
class RuntimeProductManifold : public ceres::Manifold {
 public:
  RuntimeProductManifold(std::vector<ManifoldPtr> manifolds)
      : manifolds_(std::move(manifolds)) {
    // Calculate sizes
    ambient_size_ = 0;
    tangent_size_ = 0;
    for (const auto& m : manifolds_) {
      ambient_sizes_.push_back(m->AmbientSize());
      tangent_sizes_.push_back(m->TangentSize());
      ambient_size_ += ambient_sizes_.back();
      tangent_size_ += tangent_sizes_.back();
    }
    
    // Calculate offsets
    ambient_offsets_.push_back(0);
    tangent_offsets_.push_back(0);
    for (size_t i = 0; i < manifolds_.size(); ++i) {
      ambient_offsets_.push_back(ambient_offsets_.back() + ambient_sizes_[i]);
      tangent_offsets_.push_back(tangent_offsets_.back() + tangent_sizes_[i]);
    }
  }

  int AmbientSize() const override { return ambient_size_; }
  int TangentSize() const override { return tangent_size_; }

  bool Plus(const double* x, const double* delta, double* x_plus_delta) const override {
    for (size_t i = 0; i < manifolds_.size(); ++i) {
      // Validate manifold pointer to prevent use-after-free
      if (!manifolds_[i]) {
        return false;
      }
      const double* x_i = x + ambient_offsets_[i];
      const double* delta_i = delta + tangent_offsets_[i];
      double* x_plus_delta_i = x_plus_delta + ambient_offsets_[i];
      
      if (!manifolds_[i]->Plus(x_i, delta_i, x_plus_delta_i)) {
        return false;
      }
    }
    return true;
  }

  bool Minus(const double* y, const double* x, double* y_minus_x) const override {
    for (size_t i = 0; i < manifolds_.size(); ++i) {
      // Validate manifold pointer to prevent use-after-free
      if (!manifolds_[i]) {
        return false;
      }
      const double* y_i = y + ambient_offsets_[i];
      const double* x_i = x + ambient_offsets_[i];
      double* y_minus_x_i = y_minus_x + tangent_offsets_[i];
      
      if (!manifolds_[i]->Minus(y_i, x_i, y_minus_x_i)) {
        return false;
      }
    }
    return true;
  }

  bool PlusJacobian(const double* x, double* jacobian) const override {
    // Compute Jacobian for Plus operation
    // For product manifold, Jacobian is block diagonal
    // Jacobian is AmbientSize() x TangentSize() matrix (row-major)
    // Each block is ambient_sizes_[i] x tangent_sizes_[i]
    
    // Initialize to zero
    std::fill(jacobian, jacobian + ambient_size_ * tangent_size_, 0.0);
    
    for (size_t i = 0; i < manifolds_.size(); ++i) {
      const double* x_i = x + ambient_offsets_[i];
      
      // Get sub-jacobian for this manifold
      std::vector<double> sub_jacobian(ambient_sizes_[i] * tangent_sizes_[i]);
      if (!manifolds_[i]->PlusJacobian(x_i, sub_jacobian.data())) {
        return false;
      }
      
      // Copy sub-jacobian to correct position in block diagonal
      for (int row = 0; row < ambient_sizes_[i]; ++row) {
        for (int col = 0; col < tangent_sizes_[i]; ++col) {
          jacobian[(ambient_offsets_[i] + row) * tangent_size_ + tangent_offsets_[i] + col] =
              sub_jacobian[row * tangent_sizes_[i] + col];
        }
      }
    }
    return true;
  }

  bool MinusJacobian(const double* x, double* jacobian) const override {
    // Compute Jacobian for Minus operation
    // For product manifold, Jacobian is block diagonal
    // Jacobian is TangentSize() x AmbientSize() matrix (row-major)
    // Each block is tangent_sizes_[i] x ambient_sizes_[i]
    
    // Initialize to zero
    std::fill(jacobian, jacobian + tangent_size_ * ambient_size_, 0.0);
    
    for (size_t i = 0; i < manifolds_.size(); ++i) {
      const double* x_i = x + ambient_offsets_[i];
      // Get sub-jacobian for this manifold
      std::vector<double> sub_jacobian(tangent_sizes_[i] * ambient_sizes_[i]);
      if (!manifolds_[i]->MinusJacobian(x_i, sub_jacobian.data())) {
        return false;
      }
      
      // Copy sub-jacobian to correct position in block diagonal
      for (int row = 0; row < tangent_sizes_[i]; ++row) {
        for (int col = 0; col < ambient_sizes_[i]; ++col) {
          jacobian[(tangent_offsets_[i] + row) * ambient_size_ + ambient_offsets_[i] + col] =
              sub_jacobian[row * ambient_sizes_[i] + col];
        }
      }
    }
    return true;
  }

 private:
  std::vector<ManifoldPtr> manifolds_;
  int ambient_size_;
  int tangent_size_;
  std::vector<int> ambient_sizes_;
  std::vector<int> tangent_sizes_;
  std::vector<int> ambient_offsets_;
  std::vector<int> tangent_offsets_;
};

ceres_manifold_t* ceres_wrapper_create_product_manifold(
    const ceres_manifold_t** manifolds,
    int num_manifolds) {
  if (!manifolds || num_manifolds <= 0) {
    return nullptr;
  }

  // Extract and clone Ceres manifolds from wrappers
  // We need to clone because we'll own them
  std::vector<ManifoldPtr> manifold_clones;
  for (int i = 0; i < num_manifolds; ++i) {
    if (!manifolds[i] || !manifolds[i]->manifold) {
      return nullptr;  // Invalid manifold
    }
    
    // We need to clone the manifold, but since we can't clone generically,
    // we'll just reference the original. This means the original manifold
    // must outlive the product manifold.
    // For a proper implementation, we'd need to clone based on type.
    // For now, we'll use a non-owning unique_ptr with custom deleter
    // WARNING: Non-owning reference - the original manifold wrappers must outlive
    // this product manifold. Freeing a component manifold while the product manifold
    // is alive will result in undefined behavior (dangling pointer).
    ceres::Manifold* raw_ptr = manifolds[i]->manifold.get();
    manifold_clones.push_back(ManifoldPtr(
        raw_ptr,
        [](ceres::Manifold*) {}));  // Non-owning deleter
  }

  // Create runtime product manifold
  auto* wrapped = new ceres_manifold_t;
  // Create with default deleter (owning)
  auto ptr = std::make_unique<RuntimeProductManifold>(std::move(manifold_clones));
  wrapped->manifold = ManifoldPtr(ptr.release(), [](ceres::Manifold* p) { delete p; });
  return wrapped;
}

// ============================================================================
// Problem Query Methods
// ============================================================================

int ceres_wrapper_problem_has_parameter_block(
    const ceres_problem_t* problem,
    const double* parameters) {
  if (!problem || !parameters) {
    return 0;
  }
  const auto* ceres_problem = reinterpret_cast<const ceres::Problem*>(problem);
  return ceres_problem->HasParameterBlock(parameters) ? 1 : 0;
}

int ceres_wrapper_problem_is_parameter_block_constant(
    const ceres_problem_t* problem,
    const double* parameters) {
  if (!problem || !parameters) {
    return 0;
  }
  const auto* ceres_problem = reinterpret_cast<const ceres::Problem*>(problem);
  return ceres_problem->IsParameterBlockConstant(parameters) ? 1 : 0;
}

int ceres_wrapper_problem_get_parameter_block_size(
    const ceres_problem_t* problem,
    const double* parameters) {
  if (!problem || !parameters) {
    return -1;
  }
  const auto* ceres_problem = reinterpret_cast<const ceres::Problem*>(problem);
  if (!ceres_problem->HasParameterBlock(parameters)) {
    return -1;
  }
  return ceres_problem->ParameterBlockSize(parameters);
}

int ceres_wrapper_problem_get_parameter_block_tangent_size(
    const ceres_problem_t* problem,
    const double* parameters) {
  if (!problem || !parameters) {
    return -1;
  }
  const auto* ceres_problem = reinterpret_cast<const ceres::Problem*>(problem);
  if (!ceres_problem->HasParameterBlock(parameters)) {
    return -1;
  }
  return ceres_problem->ParameterBlockTangentSize(parameters);
}

int ceres_wrapper_problem_has_manifold(
    const ceres_problem_t* problem,
    const double* parameters) {
  if (!problem || !parameters) {
    return 0;
  }
  const auto* ceres_problem = reinterpret_cast<const ceres::Problem*>(problem);
  return ceres_problem->HasManifold(parameters) ? 1 : 0;
}

ceres_manifold_t* ceres_wrapper_problem_get_manifold(
    const ceres_problem_t* problem,
    const double* parameters) {
  if (!problem || !parameters) {
    return nullptr;
  }
  const auto* ceres_problem = reinterpret_cast<const ceres::Problem*>(problem);
  const ceres::Manifold* ceres_manifold = ceres_problem->GetManifold(parameters);
  
  if (!ceres_manifold) {
    return nullptr;
  }

  // Wrap the manifold (note: we don't own it, so we create a non-owning wrapper)
  // However, since Problem owns the manifold, we need to be careful
  // For now, we'll create a wrapper that doesn't own the pointer
  // This is a limitation - the returned manifold should not be freed
  auto* wrapped = new ceres_manifold_t;
  // Create a unique_ptr that doesn't delete (since Problem owns it)
  wrapped->manifold = ManifoldPtr(
      const_cast<ceres::Manifold*>(ceres_manifold),
      [](ceres::Manifold*) {});  // No-op deleter
  return wrapped;
}

// ============================================================================
// Iteration Callback
// ============================================================================

void ceres_wrapper_solver_options_set_iteration_callback(
    ceres_solver_options_t* options,
    ceres_iteration_callback_t callback,
    void* user_data) {
  if (!options) {
    return;
  }
  
  // Clear existing callback
  options->options.callbacks.clear();
  options->callback_wrapper.reset();
  
  if (callback) {
    // Create callback wrapper and store it in options
    options->callback_wrapper = std::make_unique<IterationCallbackWrapper>(callback, user_data);
    options->options.callbacks.push_back(options->callback_wrapper.get());
  }
}

// ============================================================================
// Numeric Differentiation Cost Functions (Phase 2)
// ============================================================================

// Note: ConvertNumericDiffMethod is no longer needed since we use template specialization
// Keeping it commented out in case it's needed in the future
// static ceres::NumericDiffMethodType ConvertNumericDiffMethod(ceres_numeric_diff_method_t method) {
//   switch (method) {
//     case CERES_NUMERIC_DIFF_FORWARD:
//       return ceres::FORWARD;
//     case CERES_NUMERIC_DIFF_CENTRAL:
//       return ceres::CENTRAL;
//     case CERES_NUMERIC_DIFF_RIDDERS:
//       return ceres::RIDDERS;
//     default:
//       return ceres::CENTRAL;
//   }
// }

// Cost functor wrapper for NumericDiffCostFunction
// This functor bridges the C callback to Ceres' template-based NumericDiffCostFunction
struct NumericDiffCostFunctor {
  NumericDiffCostFunctor(ceres_numeric_diff_cost_function_t callback, void* user_data)
      : callback_(callback), user_data_(user_data) {}

  // Template operator for Ceres - converts T* to double* for C callback
  // For numeric diff, T is always double, so we can safely cast
  template <typename T>
  bool operator()(const T* const* parameters, T* residuals) const {
    // Validate callback pointer (user_data_ may legitimately be NULL)
    if (!callback_) {
      return false;
    }

    // For numeric diff, T is always double, so we can safely cast
    // Convert const T* const* to double* const* for C callback
    // Note: C callback expects double* const* (non-const pointers to const data)
    // We use const_cast which is safe here since numeric diff only reads
    double* const* params_for_callback = const_cast<double* const*>(reinterpret_cast<const double* const*>(parameters));
    double* double_residuals = reinterpret_cast<double*>(residuals);
    
    int result = callback_(params_for_callback, double_residuals, user_data_);
    
    return (result != 0);
  }

 private:
  ceres_numeric_diff_cost_function_t callback_;
  void* user_data_;
};

void* ceres_wrapper_create_numeric_diff_cost_function(
    const ceres_numeric_diff_options_t* options,
    ceres_numeric_diff_method_t method) {
  if (!options || !options->callback) {
    return nullptr;
  }

  // Create functor wrapper
  auto* functor = new NumericDiffCostFunctor(options->callback, options->user_data);
  
  // Use DynamicNumericDiffCostFunction for flexibility with variable parameter block sizes
  // The method type is a template parameter, so we need to create the appropriate specialization
  ceres::NumericDiffOptions numeric_diff_options;
  
  // Create cost function based on method type using template specialization
  ceres::CostFunction* cost_function = nullptr;
  
  switch (method) {
    case CERES_NUMERIC_DIFF_FORWARD: {
      auto* cf = new ceres::DynamicNumericDiffCostFunction<NumericDiffCostFunctor, ceres::FORWARD>(
          functor, ceres::TAKE_OWNERSHIP, numeric_diff_options);
      cf->SetNumResiduals(options->num_residuals);
      for (int i = 0; i < options->num_parameter_blocks; ++i) {
        cf->AddParameterBlock(options->parameter_block_sizes[i]);
      }
      cost_function = cf;
      break;
    }
    case CERES_NUMERIC_DIFF_CENTRAL: {
      auto* cf = new ceres::DynamicNumericDiffCostFunction<NumericDiffCostFunctor, ceres::CENTRAL>(
          functor, ceres::TAKE_OWNERSHIP, numeric_diff_options);
      cf->SetNumResiduals(options->num_residuals);
      for (int i = 0; i < options->num_parameter_blocks; ++i) {
        cf->AddParameterBlock(options->parameter_block_sizes[i]);
      }
      cost_function = cf;
      break;
    }
    case CERES_NUMERIC_DIFF_RIDDERS: {
      auto* cf = new ceres::DynamicNumericDiffCostFunction<NumericDiffCostFunctor, ceres::RIDDERS>(
          functor, ceres::TAKE_OWNERSHIP, numeric_diff_options);
      cf->SetNumResiduals(options->num_residuals);
      for (int i = 0; i < options->num_parameter_blocks; ++i) {
        cf->AddParameterBlock(options->parameter_block_sizes[i]);
      }
      cost_function = cf;
      break;
    }
    default: {
      // Default to CENTRAL
      auto* cf = new ceres::DynamicNumericDiffCostFunction<NumericDiffCostFunctor, ceres::CENTRAL>(
          functor, ceres::TAKE_OWNERSHIP, numeric_diff_options);
      cf->SetNumResiduals(options->num_residuals);
      for (int i = 0; i < options->num_parameter_blocks; ++i) {
        cf->AddParameterBlock(options->parameter_block_sizes[i]);
      }
      cost_function = cf;
      break;
    }
  }

  return cost_function;
}

void* ceres_wrapper_create_dynamic_numeric_diff_cost_function(
    const ceres_numeric_diff_options_t* options,
    ceres_numeric_diff_method_t method) {
  // Same implementation - DynamicNumericDiffCostFunction already handles dynamic sizes
  return ceres_wrapper_create_numeric_diff_cost_function(options, method);
}

void ceres_wrapper_free_numeric_diff_cost_function(void* cost_function) {
  if (cost_function) {
    // Delete the CostFunction (base class)
    // Note: The functor is owned by the cost function, so it will be deleted automatically
    delete reinterpret_cast<ceres::CostFunction*>(cost_function);
  }
}

// ============================================================================
// Additional SolverOptions (Phase 2)
// ============================================================================

void ceres_wrapper_solver_options_set_line_search_type(
    ceres_solver_options_t* options, int line_search_type) {
  if (options) {
    options->options.line_search_type = static_cast<ceres::LineSearchType>(line_search_type);
  }
}

int ceres_wrapper_solver_options_get_line_search_type(
    const ceres_solver_options_t* options) {
  return options ? static_cast<int>(options->options.line_search_type) : 0;
}

void ceres_wrapper_solver_options_set_line_search_direction_type(
    ceres_solver_options_t* options, int line_search_direction_type) {
  if (options) {
    options->options.line_search_direction_type =
        static_cast<ceres::LineSearchDirectionType>(line_search_direction_type);
  }
}

int ceres_wrapper_solver_options_get_line_search_direction_type(
    const ceres_solver_options_t* options) {
  return options ? static_cast<int>(options->options.line_search_direction_type) : 0;
}

void ceres_wrapper_solver_options_set_nonlinear_conjugate_gradient_type(
    ceres_solver_options_t* options, int ncg_type) {
  if (options) {
    options->options.nonlinear_conjugate_gradient_type =
        static_cast<ceres::NonlinearConjugateGradientType>(ncg_type);
  }
}

int ceres_wrapper_solver_options_get_nonlinear_conjugate_gradient_type(
    const ceres_solver_options_t* options) {
  return options ? static_cast<int>(options->options.nonlinear_conjugate_gradient_type) : 0;
}

void ceres_wrapper_solver_options_set_max_lbfgs_rank(
    ceres_solver_options_t* options, int max_lbfgs_rank) {
  if (options) {
    options->options.max_lbfgs_rank = max_lbfgs_rank;
  }
}

int ceres_wrapper_solver_options_get_max_lbfgs_rank(
    const ceres_solver_options_t* options) {
  return options ? options->options.max_lbfgs_rank : 0;
}

void ceres_wrapper_solver_options_set_max_line_search_step_contraction(
    ceres_solver_options_t* options, double max_step_contraction) {
  if (options) {
    options->options.max_line_search_step_contraction = max_step_contraction;
  }
}

double ceres_wrapper_solver_options_get_max_line_search_step_contraction(
    const ceres_solver_options_t* options) {
  return options ? options->options.max_line_search_step_contraction : 0.0;
}

void ceres_wrapper_solver_options_set_min_line_search_step_contraction(
    ceres_solver_options_t* options, double min_step_contraction) {
  if (options) {
    options->options.min_line_search_step_contraction = min_step_contraction;
  }
}

double ceres_wrapper_solver_options_get_min_line_search_step_contraction(
    const ceres_solver_options_t* options) {
  return options ? options->options.min_line_search_step_contraction : 0.0;
}

void ceres_wrapper_solver_options_set_max_num_line_search_step_size_iterations(
    ceres_solver_options_t* options, int max_iterations) {
  if (options) {
    options->options.max_num_line_search_step_size_iterations = max_iterations;
  }
}

int ceres_wrapper_solver_options_get_max_num_line_search_step_size_iterations(
    const ceres_solver_options_t* options) {
  return options ? options->options.max_num_line_search_step_size_iterations : 0;
}

void ceres_wrapper_solver_options_set_max_num_line_search_direction_restarts(
    ceres_solver_options_t* options, int max_restarts) {
  if (options) {
    options->options.max_num_line_search_direction_restarts = max_restarts;
  }
}

int ceres_wrapper_solver_options_get_max_num_line_search_direction_restarts(
    const ceres_solver_options_t* options) {
  return options ? options->options.max_num_line_search_direction_restarts : 0;
}

void ceres_wrapper_solver_options_set_line_search_sufficient_function_decrease(
    ceres_solver_options_t* options, double sufficient_decrease) {
  if (options) {
    options->options.line_search_sufficient_function_decrease = sufficient_decrease;
  }
}

double ceres_wrapper_solver_options_get_line_search_sufficient_function_decrease(
    const ceres_solver_options_t* options) {
  return options ? options->options.line_search_sufficient_function_decrease : 0.0;
}

void ceres_wrapper_solver_options_set_line_search_sufficient_curvature_decrease(
    ceres_solver_options_t* options, double sufficient_curvature_decrease) {
  if (options) {
    options->options.line_search_sufficient_curvature_decrease = sufficient_curvature_decrease;
  }
}

double ceres_wrapper_solver_options_get_line_search_sufficient_curvature_decrease(
    const ceres_solver_options_t* options) {
  return options ? options->options.line_search_sufficient_curvature_decrease : 0.0;
}

void ceres_wrapper_solver_options_set_max_line_search_step_expansion(
    ceres_solver_options_t* options, double max_step_expansion) {
  if (options) {
    options->options.max_line_search_step_expansion = max_step_expansion;
  }
}

double ceres_wrapper_solver_options_get_max_line_search_step_expansion(
    const ceres_solver_options_t* options) {
  return options ? options->options.max_line_search_step_expansion : 0.0;
}

void ceres_wrapper_solver_options_set_max_lm_diagonal(
    ceres_solver_options_t* options, double max_lm_diagonal) {
  if (options) {
    options->options.max_lm_diagonal = max_lm_diagonal;
  }
}

double ceres_wrapper_solver_options_get_max_lm_diagonal(
    const ceres_solver_options_t* options) {
  return options ? options->options.max_lm_diagonal : 0.0;
}

void ceres_wrapper_solver_options_set_min_lm_diagonal(
    ceres_solver_options_t* options, double min_lm_diagonal) {
  if (options) {
    options->options.min_lm_diagonal = min_lm_diagonal;
  }
}

double ceres_wrapper_solver_options_get_min_lm_diagonal(
    const ceres_solver_options_t* options) {
  return options ? options->options.min_lm_diagonal : 0.0;
}

void ceres_wrapper_solver_options_set_sparse_linear_algebra_library_type(
    ceres_solver_options_t* options, int sparse_library_type) {
  if (options) {
    options->options.sparse_linear_algebra_library_type =
        static_cast<ceres::SparseLinearAlgebraLibraryType>(sparse_library_type);
  }
}

int ceres_wrapper_solver_options_get_sparse_linear_algebra_library_type(
    const ceres_solver_options_t* options) {
  return options ? static_cast<int>(options->options.sparse_linear_algebra_library_type) : 0;
}

void ceres_wrapper_solver_options_set_dense_linear_algebra_library_type(
    ceres_solver_options_t* options, int dense_library_type) {
  if (options) {
    options->options.dense_linear_algebra_library_type =
        static_cast<ceres::DenseLinearAlgebraLibraryType>(dense_library_type);
  }
}

int ceres_wrapper_solver_options_get_dense_linear_algebra_library_type(
    const ceres_solver_options_t* options) {
  return options ? static_cast<int>(options->options.dense_linear_algebra_library_type) : 0;
}

void ceres_wrapper_solver_options_set_max_linear_solver_iterations(
    ceres_solver_options_t* options, int max_iterations) {
  if (options) {
    options->options.max_linear_solver_iterations = max_iterations;
  }
}

int ceres_wrapper_solver_options_get_max_linear_solver_iterations(
    const ceres_solver_options_t* options) {
  return options ? options->options.max_linear_solver_iterations : 0;
}

void ceres_wrapper_solver_options_set_min_linear_solver_iterations(
    ceres_solver_options_t* options, int min_iterations) {
  if (options) {
    options->options.min_linear_solver_iterations = min_iterations;
  }
}

int ceres_wrapper_solver_options_get_min_linear_solver_iterations(
    const ceres_solver_options_t* options) {
  return options ? options->options.min_linear_solver_iterations : 0;
}

// Note: linear_solver_tolerance doesn't exist in Ceres Solver::Options
// This is a placeholder - remove or comment out if not available
void ceres_wrapper_solver_options_set_linear_solver_tolerance(
    ceres_solver_options_t* options, double tolerance) {
  // This option doesn't exist in Ceres - no-op
  (void)options;
  (void)tolerance;
}

double ceres_wrapper_solver_options_get_linear_solver_tolerance(
    const ceres_solver_options_t* options) {
  // This option doesn't exist in Ceres - return 0
  (void)options;
  return 0.0;
}

void ceres_wrapper_solver_options_set_use_inner_iterations(
    ceres_solver_options_t* options, int use_inner_iterations) {
  if (options) {
    options->options.use_inner_iterations = (use_inner_iterations != 0);
  }
}

int ceres_wrapper_solver_options_get_use_inner_iterations(
    const ceres_solver_options_t* options) {
  return options ? (options->options.use_inner_iterations ? 1 : 0) : 0;
}

void ceres_wrapper_solver_options_set_inner_iteration_tolerance(
    ceres_solver_options_t* options, double tolerance) {
  if (options) {
    options->options.inner_iteration_tolerance = tolerance;
  }
}

double ceres_wrapper_solver_options_get_inner_iteration_tolerance(
    const ceres_solver_options_t* options) {
  return options ? options->options.inner_iteration_tolerance : 0.0;
}

void ceres_wrapper_solver_options_set_max_solver_time_in_seconds(
    ceres_solver_options_t* options, double max_time) {
  if (options) {
    options->options.max_solver_time_in_seconds = max_time;
  }
}

double ceres_wrapper_solver_options_get_max_solver_time_in_seconds(
    const ceres_solver_options_t* options) {
  return options ? options->options.max_solver_time_in_seconds : 0.0;
}

void ceres_wrapper_solver_options_set_update_state_every_iteration(
    ceres_solver_options_t* options, int update_every_iteration) {
  if (options) {
    options->options.update_state_every_iteration = (update_every_iteration != 0);
  }
}

int ceres_wrapper_solver_options_get_update_state_every_iteration(
    const ceres_solver_options_t* options) {
  return options ? (options->options.update_state_every_iteration ? 1 : 0) : 0;
}

// ============================================================================
// Problem Options (Phase 2)
// ============================================================================

struct ceres_problem_options_s {
  ceres::Problem::Options options;
};

// Evaluation callback wrapper class
// Note: We need to store the wrapper to keep it alive
// We'll use a map to track callbacks per options object
// Thread-safe: protected by mutex
static std::mutex evaluation_callbacks_mutex;
static std::map<ceres_problem_options_t*, std::unique_ptr<ceres::EvaluationCallback>> evaluation_callbacks;

class EvaluationCallbackWrapper : public ceres::EvaluationCallback {
 public:
  EvaluationCallbackWrapper(
      ceres_evaluation_callback_t callback,
      void* user_data)
      : callback_(callback), user_data_(user_data) {}

  void PrepareForEvaluation(bool evaluate_jacobians,
                            bool new_evaluation_point) override {
    (void)evaluate_jacobians;
    (void)new_evaluation_point;
    // Validate callback and user_data_ to prevent use-after-free
    if (callback_ && user_data_) {
      // Note: We don't have num_residuals and parameter_block_sizes here
      // This is a limitation - we'll call with dummy values
      // In practice, the callback should be set up with proper context
      callback_(user_data_, 0, 0, nullptr);
    }
  }

 private:
  ceres_evaluation_callback_t callback_;
  void* user_data_;
};

ceres_problem_options_t* ceres_wrapper_create_problem_options() {
  return new ceres_problem_options_t;
}

void ceres_wrapper_free_problem_options(ceres_problem_options_t* options) {
  // Remove callback from map if it exists (thread-safe)
  {
    std::lock_guard<std::mutex> lock(evaluation_callbacks_mutex);
    evaluation_callbacks.erase(options);
  }
  delete options;
}

void ceres_wrapper_problem_options_set_cost_function_ownership(
    ceres_problem_options_t* options, int ownership) {
  if (options) {
    options->options.cost_function_ownership =
        static_cast<ceres::Ownership>(ownership);
  }
}

int ceres_wrapper_problem_options_get_cost_function_ownership(
    const ceres_problem_options_t* options) {
  return options ? static_cast<int>(options->options.cost_function_ownership) : 0;
}

void ceres_wrapper_problem_options_set_loss_function_ownership(
    ceres_problem_options_t* options, int ownership) {
  if (options) {
    options->options.loss_function_ownership =
        static_cast<ceres::Ownership>(ownership);
  }
}

int ceres_wrapper_problem_options_get_loss_function_ownership(
    const ceres_problem_options_t* options) {
  return options ? static_cast<int>(options->options.loss_function_ownership) : 0;
}

void ceres_wrapper_problem_options_set_manifold_ownership(
    ceres_problem_options_t* options, int ownership) {
  if (options) {
    options->options.manifold_ownership =
        static_cast<ceres::Ownership>(ownership);
  }
}

int ceres_wrapper_problem_options_get_manifold_ownership(
    const ceres_problem_options_t* options) {
  return options ? static_cast<int>(options->options.manifold_ownership) : 0;
}

void ceres_wrapper_problem_options_set_enable_fast_removal(
    ceres_problem_options_t* options, int enable) {
  if (options) {
    options->options.enable_fast_removal = (enable != 0);
  }
}

int ceres_wrapper_problem_options_get_enable_fast_removal(
    const ceres_problem_options_t* options) {
  return options ? (options->options.enable_fast_removal ? 1 : 0) : 0;
}

void ceres_wrapper_problem_options_set_disable_all_safety_checks(
    ceres_problem_options_t* options, int disable) {
  if (options) {
    options->options.disable_all_safety_checks = (disable != 0);
  }
}

int ceres_wrapper_problem_options_get_disable_all_safety_checks(
    const ceres_problem_options_t* options) {
  return options ? (options->options.disable_all_safety_checks ? 1 : 0) : 0;
}

void ceres_wrapper_problem_options_set_evaluation_callback(
    ceres_problem_options_t* options,
    ceres_evaluation_callback_t callback,
    void* user_data) {
  if (options) {
    std::lock_guard<std::mutex> lock(evaluation_callbacks_mutex);
    if (callback) {
      // Create wrapper and store it in the map (thread-safe)
      evaluation_callbacks[options] = std::make_unique<EvaluationCallbackWrapper>(callback, user_data);
      options->options.evaluation_callback = evaluation_callbacks[options].get();
    } else {
      evaluation_callbacks.erase(options);
      options->options.evaluation_callback = nullptr;
    }
  }
}

ceres_problem_t* ceres_wrapper_create_problem_with_options(
    const ceres_problem_options_t* options) {
  if (!options) {
    return reinterpret_cast<ceres_problem_t*>(new ceres::Problem);
  }
  return reinterpret_cast<ceres_problem_t*>(new ceres::Problem(options->options));
}

// ============================================================================
// Parameter Bounds (Phase 2)
// ============================================================================

void ceres_wrapper_problem_set_parameter_lower_bound(
    ceres_problem_t* problem,
    double* parameters,
    int index,
    double lower_bound) {
  if (problem) {
    reinterpret_cast<ceres::Problem*>(problem)->SetParameterLowerBound(parameters, index, lower_bound);
  }
}

void ceres_wrapper_problem_set_parameter_upper_bound(
    ceres_problem_t* problem,
    double* parameters,
    int index,
    double upper_bound) {
  if (problem) {
    reinterpret_cast<ceres::Problem*>(problem)->SetParameterUpperBound(parameters, index, upper_bound);
  }
}

double ceres_wrapper_problem_get_parameter_lower_bound(
    const ceres_problem_t* problem,
    const double* parameters,
    int index) {
  if (!problem) {
    return -std::numeric_limits<double>::infinity();
  }
  return reinterpret_cast<const ceres::Problem*>(problem)->GetParameterLowerBound(parameters, index);
}

double ceres_wrapper_problem_get_parameter_upper_bound(
    const ceres_problem_t* problem,
    const double* parameters,
    int index) {
  if (!problem) {
    return std::numeric_limits<double>::infinity();
  }
  return reinterpret_cast<const ceres::Problem*>(problem)->GetParameterUpperBound(parameters, index);
}

// ============================================================================
// Additional SolverSummary Fields (Phase 2)
// ============================================================================
// Note: Timing functions are already defined above (lines 355-378)
// Only add new functions that don't exist yet

int ceres_wrapper_solver_summary_get_num_parameter_blocks(
    const ceres_solver_summary_t* summary) {
  return summary ? summary->summary.num_parameter_blocks : 0;
}

int ceres_wrapper_solver_summary_get_num_parameters(
    const ceres_solver_summary_t* summary) {
  return summary ? summary->summary.num_parameters : 0;
}

int ceres_wrapper_solver_summary_get_num_effective_parameters(
    const ceres_solver_summary_t* summary) {
  return summary ? summary->summary.num_effective_parameters : 0;
}

int ceres_wrapper_solver_summary_get_num_residual_blocks(
    const ceres_solver_summary_t* summary) {
  return summary ? summary->summary.num_residual_blocks : 0;
}

int ceres_wrapper_solver_summary_get_num_residuals(
    const ceres_solver_summary_t* summary) {
  return summary ? summary->summary.num_residuals : 0;
}

double ceres_wrapper_solver_summary_get_cost_change(
    const ceres_solver_summary_t* summary) {
  if (!summary) {
    return 0.0;
  }
  return summary->summary.initial_cost - summary->summary.final_cost;
}

// ============================================================================
// Covariance Estimation (Phase 3 - HIGH Priority)
// ============================================================================

struct ceres_covariance_options_s {
  ceres::Covariance::Options options;
};

ceres_covariance_options_t* ceres_wrapper_create_covariance_options() {
  return new ceres_covariance_options_t;
}

void ceres_wrapper_free_covariance_options(ceres_covariance_options_t* options) {
  delete options;
}

void ceres_wrapper_covariance_options_set_num_threads(
    ceres_covariance_options_t* options, int num_threads) {
  if (options) {
    options->options.num_threads = num_threads;
  }
}

int ceres_wrapper_covariance_options_get_num_threads(
    const ceres_covariance_options_t* options) {
  return options ? options->options.num_threads : 0;
}

void ceres_wrapper_covariance_options_set_sparse_linear_algebra_library_type(
    ceres_covariance_options_t* options, int library_type) {
  if (options) {
    options->options.sparse_linear_algebra_library_type =
        static_cast<ceres::SparseLinearAlgebraLibraryType>(library_type);
  }
}

int ceres_wrapper_covariance_options_get_sparse_linear_algebra_library_type(
    const ceres_covariance_options_t* options) {
  return options ? static_cast<int>(options->options.sparse_linear_algebra_library_type) : 0;
}

void ceres_wrapper_covariance_options_set_algorithm_type(
    ceres_covariance_options_t* options, int algorithm_type) {
  if (options) {
    options->options.algorithm_type =
        static_cast<ceres::CovarianceAlgorithmType>(algorithm_type);
  }
}

int ceres_wrapper_covariance_options_get_algorithm_type(
    const ceres_covariance_options_t* options) {
  return options ? static_cast<int>(options->options.algorithm_type) : 0;
}

void ceres_wrapper_covariance_options_set_min_reciprocal_condition_number(
    ceres_covariance_options_t* options, double min_reciprocal_condition_number) {
  if (options) {
    options->options.min_reciprocal_condition_number = min_reciprocal_condition_number;
  }
}

double ceres_wrapper_covariance_options_get_min_reciprocal_condition_number(
    const ceres_covariance_options_t* options) {
  return options ? options->options.min_reciprocal_condition_number : 0.0;
}

void ceres_wrapper_covariance_options_set_null_space_rank(
    ceres_covariance_options_t* options, int null_space_rank) {
  if (options) {
    options->options.null_space_rank = null_space_rank;
  }
}

int ceres_wrapper_covariance_options_get_null_space_rank(
    const ceres_covariance_options_t* options) {
  return options ? options->options.null_space_rank : 0;
}

void ceres_wrapper_covariance_options_set_apply_loss_function(
    ceres_covariance_options_t* options, int apply_loss_function) {
  if (options) {
    options->options.apply_loss_function = (apply_loss_function != 0);
  }
}

int ceres_wrapper_covariance_options_get_apply_loss_function(
    const ceres_covariance_options_t* options) {
  return options ? (options->options.apply_loss_function ? 1 : 0) : 0;
}

struct ceres_covariance_s {
  std::unique_ptr<ceres::Covariance> covariance;
  
  // Constructor with default options
  ceres_covariance_s() : covariance(std::make_unique<ceres::Covariance>(ceres::Covariance::Options())) {}
  
  // Constructor with custom options
  ceres_covariance_s(const ceres::Covariance::Options& options) : covariance(std::make_unique<ceres::Covariance>(options)) {}
};

ceres_covariance_t* ceres_wrapper_create_covariance() {
  return new ceres_covariance_t;
}

// Create covariance with options
ceres_covariance_t* ceres_wrapper_create_covariance_with_options(
    const ceres_covariance_options_t* options) {
  if (!options) {
    return new ceres_covariance_t;
  }
  return new ceres_covariance_t(options->options);
}

void ceres_wrapper_free_covariance(ceres_covariance_t* covariance) {
  delete covariance;
}

ceres_wrapper_error_code_t ceres_wrapper_covariance_compute(
    ceres_covariance_t* covariance,
    const ceres_problem_t* problem,
    const ceres_covariance_options_t* options,
    const double** parameter_blocks,
    int num_parameter_blocks,
    char* error_message,
    int error_message_size) {
  if (!covariance) {
    if (error_message && error_message_size > 0) {
      std::strncpy(error_message, "covariance is null", error_message_size - 1);
      error_message[error_message_size - 1] = '\0';
    }
    return CERES_WRAPPER_ERROR_NULL_POINTER;
  }
  if (!problem) {
    if (error_message && error_message_size > 0) {
      std::strncpy(error_message, "problem is null", error_message_size - 1);
      error_message[error_message_size - 1] = '\0';
    }
    return CERES_WRAPPER_ERROR_NULL_POINTER;
  }
  if (!parameter_blocks) {
    if (error_message && error_message_size > 0) {
      std::strncpy(error_message, "parameter_blocks is null", error_message_size - 1);
      error_message[error_message_size - 1] = '\0';
    }
    return CERES_WRAPPER_ERROR_NULL_POINTER;
  }
  if (num_parameter_blocks <= 0) {
    if (error_message && error_message_size > 0) {
      std::strncpy(error_message, "num_parameter_blocks must be positive", error_message_size - 1);
      error_message[error_message_size - 1] = '\0';
    }
    return CERES_WRAPPER_ERROR_INVALID_PARAMETER;
  }

  try {
    // Covariance::Compute requires non-const Problem*, so we need to cast
    ceres::Problem* ceres_problem = const_cast<ceres::Problem*>(
        reinterpret_cast<const ceres::Problem*>(problem));
    std::vector<const double*> param_blocks(parameter_blocks, parameter_blocks + num_parameter_blocks);
    
    // Note: Options are set when creating covariance object
    // If different options are needed, user should create a new covariance object
    (void)options;  // Options parameter kept for API compatibility but not used here
    
    bool success = covariance->covariance->Compute(param_blocks, ceres_problem);
    
    if (!success) {
      if (error_message && error_message_size > 0) {
        std::strncpy(error_message, "Covariance computation failed", error_message_size - 1);
        error_message[error_message_size - 1] = '\0';
      }
      return CERES_WRAPPER_ERROR_INVALID_OPERATION;
    }
    
    return CERES_WRAPPER_SUCCESS;
  } catch (const std::exception& e) {
    if (error_message && error_message_size > 0) {
      std::strncpy(error_message, e.what(), error_message_size - 1);
      error_message[error_message_size - 1] = '\0';
    }
    return CERES_WRAPPER_ERROR_EXCEPTION;
  } catch (...) {
    if (error_message && error_message_size > 0) {
      std::strncpy(error_message, "Unknown exception during covariance computation", error_message_size - 1);
      error_message[error_message_size - 1] = '\0';
    }
    return CERES_WRAPPER_ERROR_EXCEPTION;
  }
}

ceres_wrapper_error_code_t ceres_wrapper_covariance_get_covariance_block(
    const ceres_covariance_t* covariance,
    const double* parameter_block1,
    const double* parameter_block2,
    double* covariance_block,
    char* error_message,
    int error_message_size) {
  if (!covariance) {
    if (error_message && error_message_size > 0) {
      std::strncpy(error_message, "covariance is null", error_message_size - 1);
      error_message[error_message_size - 1] = '\0';
    }
    return CERES_WRAPPER_ERROR_NULL_POINTER;
  }
  if (!parameter_block1) {
    if (error_message && error_message_size > 0) {
      std::strncpy(error_message, "parameter_block1 is null", error_message_size - 1);
      error_message[error_message_size - 1] = '\0';
    }
    return CERES_WRAPPER_ERROR_NULL_POINTER;
  }
  if (!parameter_block2) {
    if (error_message && error_message_size > 0) {
      std::strncpy(error_message, "parameter_block2 is null", error_message_size - 1);
      error_message[error_message_size - 1] = '\0';
    }
    return CERES_WRAPPER_ERROR_NULL_POINTER;
  }
  if (!covariance_block) {
    if (error_message && error_message_size > 0) {
      std::strncpy(error_message, "covariance_block is null", error_message_size - 1);
      error_message[error_message_size - 1] = '\0';
    }
    return CERES_WRAPPER_ERROR_NULL_POINTER;
  }

  try {
    // GetCovarianceBlock requires the covariance_block array to be pre-allocated
    // with size = block1_size * block2_size
    // Caller is responsible for providing correctly sized array
    bool success = covariance->covariance->GetCovarianceBlock(
        parameter_block1, parameter_block2, covariance_block);
    
    if (!success) {
      if (error_message && error_message_size > 0) {
        std::strncpy(error_message, "Failed to get covariance block", error_message_size - 1);
        error_message[error_message_size - 1] = '\0';
      }
      return CERES_WRAPPER_ERROR_NOT_FOUND;
    }
    
    return CERES_WRAPPER_SUCCESS;
  } catch (const std::exception& e) {
    if (error_message && error_message_size > 0) {
      std::strncpy(error_message, e.what(), error_message_size - 1);
      error_message[error_message_size - 1] = '\0';
    }
    return CERES_WRAPPER_ERROR_EXCEPTION;
  } catch (...) {
    if (error_message && error_message_size > 0) {
      std::strncpy(error_message, "Unknown exception", error_message_size - 1);
      error_message[error_message_size - 1] = '\0';
    }
    return CERES_WRAPPER_ERROR_EXCEPTION;
  }
}

ceres_wrapper_error_code_t ceres_wrapper_covariance_get_covariance_matrix(
    const ceres_covariance_t* covariance,
    const double** parameter_blocks,
    int num_parameter_blocks,
    double* covariance_matrix,
    char* error_message,
    int error_message_size) {
  if (!covariance) {
    if (error_message && error_message_size > 0) {
      std::strncpy(error_message, "covariance is null", error_message_size - 1);
      error_message[error_message_size - 1] = '\0';
    }
    return CERES_WRAPPER_ERROR_NULL_POINTER;
  }
  if (!parameter_blocks) {
    if (error_message && error_message_size > 0) {
      std::strncpy(error_message, "parameter_blocks is null", error_message_size - 1);
      error_message[error_message_size - 1] = '\0';
    }
    return CERES_WRAPPER_ERROR_NULL_POINTER;
  }
  if (!covariance_matrix) {
    if (error_message && error_message_size > 0) {
      std::strncpy(error_message, "covariance_matrix is null", error_message_size - 1);
      error_message[error_message_size - 1] = '\0';
    }
    return CERES_WRAPPER_ERROR_NULL_POINTER;
  }
  if (num_parameter_blocks <= 0) {
    if (error_message && error_message_size > 0) {
      std::strncpy(error_message, "num_parameter_blocks must be positive", error_message_size - 1);
      error_message[error_message_size - 1] = '\0';
    }
    return CERES_WRAPPER_ERROR_INVALID_PARAMETER;
  }

  try {
    std::vector<const double*> param_blocks(parameter_blocks, parameter_blocks + num_parameter_blocks);
    
    bool success = covariance->covariance->GetCovarianceMatrix(
        param_blocks, covariance_matrix);
    
    if (!success) {
      if (error_message && error_message_size > 0) {
        std::strncpy(error_message, "Failed to get covariance matrix", error_message_size - 1);
        error_message[error_message_size - 1] = '\0';
      }
      return CERES_WRAPPER_ERROR_INVALID_OPERATION;
    }
    
    return CERES_WRAPPER_SUCCESS;
  } catch (const std::exception& e) {
    if (error_message && error_message_size > 0) {
      std::strncpy(error_message, e.what(), error_message_size - 1);
      error_message[error_message_size - 1] = '\0';
    }
    return CERES_WRAPPER_ERROR_EXCEPTION;
  } catch (...) {
    if (error_message && error_message_size > 0) {
      std::strncpy(error_message, "Unknown exception", error_message_size - 1);
      error_message[error_message_size - 1] = '\0';
    }
    return CERES_WRAPPER_ERROR_EXCEPTION;
  }
}

// ============================================================================
// Gradient Checker (Phase 3 - MEDIUM Priority)
// ============================================================================

struct ceres_gradient_checker_options_s {
  ceres::NumericDiffOptions numeric_diff_options;
  double gradient_check_relative_precision;  // Stored separately, used in Probe()
};

ceres_gradient_checker_options_t* ceres_wrapper_create_gradient_checker_options() {
  auto* opts = new ceres_gradient_checker_options_s;
  opts->gradient_check_relative_precision = 1e-4;  // Default value
  return opts;
}

void ceres_wrapper_free_gradient_checker_options(ceres_gradient_checker_options_t* options) {
  delete options;
}

void ceres_wrapper_gradient_checker_options_set_gradient_check_relative_precision(
    ceres_gradient_checker_options_t* options, double precision) {
  if (options) {
    options->gradient_check_relative_precision = precision;
  }
}

double ceres_wrapper_gradient_checker_options_get_gradient_check_relative_precision(
    const ceres_gradient_checker_options_t* options) {
  if (!options) return 0.0;
  return options->gradient_check_relative_precision;
}

void ceres_wrapper_gradient_checker_options_set_gradient_check_numeric_derivative_relative_step_size(
    ceres_gradient_checker_options_t* options, double step_size) {
  if (options) {
    options->numeric_diff_options.relative_step_size = step_size;
  }
}

double ceres_wrapper_gradient_checker_options_get_gradient_check_numeric_derivative_relative_step_size(
    const ceres_gradient_checker_options_t* options) {
  if (!options) return 0.0;
  return options->numeric_diff_options.relative_step_size;
}

struct ceres_gradient_checker_s {
  std::unique_ptr<ceres::GradientChecker> checker;
};

ceres_gradient_checker_t* ceres_wrapper_create_gradient_checker(
    void* cost_function,
    const ceres_manifold_t** manifolds,
    int num_manifolds,
    const ceres_gradient_checker_options_t* options) {
  if (!cost_function) {
    return nullptr;
  }
  
  try {
    ceres::CostFunction* ceres_cost_function = reinterpret_cast<ceres::CostFunction*>(cost_function);
    
    std::vector<const ceres::Manifold*> ceres_manifolds;
    if (manifolds && num_manifolds > 0) {
      for (int i = 0; i < num_manifolds; ++i) {
        if (manifolds[i]) {
          ceres_manifolds.push_back(manifolds[i]->manifold.get());
        }
      }
    }
    
    ceres::NumericDiffOptions numeric_diff_options;
    if (options) {
      numeric_diff_options = options->numeric_diff_options;
    }
    
    // GradientChecker constructor takes pointer to vector or nullptr
    const std::vector<const ceres::Manifold*>* manifolds_ptr = 
        ceres_manifolds.empty() ? nullptr : &ceres_manifolds;
    
    auto* checker = new ceres_gradient_checker_s;
    checker->checker = std::make_unique<ceres::GradientChecker>(
        ceres_cost_function, manifolds_ptr, numeric_diff_options);
    
    return checker;
  } catch (...) {
    return nullptr;
  }
}

void ceres_wrapper_free_gradient_checker(ceres_gradient_checker_t* checker) {
  delete checker;
}

ceres_wrapper_error_code_t ceres_wrapper_gradient_checker_probe(
    const ceres_gradient_checker_t* checker,
    const double* const* parameters,
    double relative_precision,
    char* error_message,
    int error_message_size) {
  if (!checker) {
    if (error_message && error_message_size > 0) {
      std::strncpy(error_message, "checker is null", error_message_size - 1);
      error_message[error_message_size - 1] = '\0';
    }
    return CERES_WRAPPER_ERROR_NULL_POINTER;
  }
  if (!parameters) {
    if (error_message && error_message_size > 0) {
      std::strncpy(error_message, "parameters is null", error_message_size - 1);
      error_message[error_message_size - 1] = '\0';
    }
    return CERES_WRAPPER_ERROR_NULL_POINTER;
  }
  
  try {
    ceres::GradientChecker::ProbeResults results;
    bool success = checker->checker->Probe(parameters, relative_precision, &results);
    
    if (error_message && error_message_size > 0) {
      if (!results.error_log.empty()) {
        int copy_size = std::min(static_cast<int>(results.error_log.length()), error_message_size - 1);
        std::strncpy(error_message, results.error_log.c_str(), copy_size);
        error_message[copy_size] = '\0';
      } else if (success) {
        error_message[0] = '\0';
      } else {
        std::strncpy(error_message, "Gradient check failed", error_message_size - 1);
        error_message[error_message_size - 1] = '\0';
      }
    }
    
    if (!success) {
      return CERES_WRAPPER_ERROR_INVALID_PARAMETER;  // Gradients don't match
    }
    
    return CERES_WRAPPER_SUCCESS;
  } catch (const std::exception& e) {
    if (error_message && error_message_size > 0) {
      std::strncpy(error_message, e.what(), error_message_size - 1);
      error_message[error_message_size - 1] = '\0';
    }
    return CERES_WRAPPER_ERROR_EXCEPTION;
  } catch (...) {
    if (error_message && error_message_size > 0) {
      std::strncpy(error_message, "Unknown exception during gradient check", error_message_size - 1);
      error_message[error_message_size - 1] = '\0';
    }
    return CERES_WRAPPER_ERROR_EXCEPTION;
  }
}

// ============================================================================
// Context (Phase 3 - MEDIUM Priority)
// ============================================================================

struct ceres_context_s {
  ceres::Context* context;
  
  ceres_context_s() : context(ceres::Context::Create()) {}
  
  ~ceres_context_s() {
    delete context;
  }
};

ceres_context_t* ceres_wrapper_create_context() {
  return new ceres_context_s;
}

void ceres_wrapper_free_context(ceres_context_t* context) {
  delete context;
}

void ceres_wrapper_problem_options_set_context(
    ceres_problem_options_t* options,
    ceres_context_t* context) {
  if (options && context) {
    // Context::Create() returns ContextImpl* which is the correct type
    options->options.context = context->context;
  }
}

// ============================================================================
// Cubic Interpolator (1D) (Phase 3 - MEDIUM Priority)
// ============================================================================

// Adapter for 1D array data to work with Ceres CubicInterpolator
class Array1DAdapter {
 public:
  enum { DATA_DIMENSION = 1 };

  Array1DAdapter(const double* data, int size)
      : data_(data), size_(size) {}

  void GetValue(const int index, double* const value) const {
    if (index < 0 || index >= size_) {
      *value = 0.0;  // Out of bounds - return default value
    } else {
      *value = data_[index];
    }
  }

  int NumValues() const { return size_; }

 private:
  const double* data_;
  int size_;
};

struct ceres_cubic_interpolator_s {
  std::unique_ptr<ceres::CubicInterpolator<Array1DAdapter>> interpolator;
  std::unique_ptr<Array1DAdapter> adapter;
  std::vector<double> data;  // Keep data alive
};

ceres_cubic_interpolator_t* ceres_wrapper_create_cubic_interpolator(
    const double* data,
    int num_values) {
  if (!data || num_values <= 0) {
    return nullptr;
  }
  
  auto* interp = new ceres_cubic_interpolator_t;
  
  // Copy data to keep it alive
  interp->data.assign(data, data + num_values);
  
  // Create adapter with copied data
  interp->adapter = std::make_unique<Array1DAdapter>(
      interp->data.data(), num_values);
  
  // Create interpolator with adapter
  interp->interpolator = std::make_unique<ceres::CubicInterpolator<Array1DAdapter>>(
      *interp->adapter);
  
  return interp;
}

void ceres_wrapper_free_cubic_interpolator(
    ceres_cubic_interpolator_t* interpolator) {
  delete interpolator;
}

void ceres_wrapper_cubic_interpolator_evaluate(
    const ceres_cubic_interpolator_t* interpolator,
    double x,
    double* value,
    double* gradient) {
  if (!interpolator) {
    if (value) *value = 0.0;
    if (gradient) *gradient = 0.0;
    return;
  }
  
  double val = 0.0;
  double grad = 0.0;
  
  // Evaluate at index x (clamped to valid range)
  double clamped_x = std::max(0.0, std::min(x, static_cast<double>(interpolator->adapter->NumValues() - 1)));
  interpolator->interpolator->Evaluate(clamped_x, &val, &grad);
  
  if (value) *value = val;
  if (gradient) *gradient = grad;
}