#include "gp4c.h"
#include <math.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_errno.h>
#include <stdio.h>

#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif

/**
 * Helper: Cholesky decomposition with jitter fallback
 *
 * Tries Cholesky decomposition first. If it fails (e.g., due to singularity
 * at x_g=0 where g(0)=0 is deterministic), adds increasing jitter to the
 * diagonal and retries.
 *
 * @param K        Matrix to decompose (modified in place)
 * @param K_copy   Backup copy of K (for retry with more jitter)
 * @param n        Matrix dimension
 * @return 0 on success, non-zero on failure after all attempts
 */
static int cholesky_with_fallback(gsl_matrix *K, gsl_matrix *K_copy, int n) {
    // Disable GSL error handler temporarily (it calls abort() by default)
    gsl_error_handler_t *old_handler = gsl_set_error_handler_off();

    // First attempt: try Cholesky on the original matrix
    int status = gsl_linalg_cholesky_decomp1(K);
    if (status == 0) {
        gsl_set_error_handler(old_handler);
        return 0;  // Success on first try
    }

    // Cholesky failed - try adding increasing jitter
    double jitter = GP_JITTER_FALLBACK;
    for (int attempt = 1; attempt <= GP_JITTER_MAX_ATTEMPTS; attempt++) {
        // Restore K from backup and add jitter
        gsl_matrix_memcpy(K, K_copy);
        for (int i = 0; i < n; i++) {
            double diag_val = gsl_matrix_get(K, i, i);
            gsl_matrix_set(K, i, i, diag_val + jitter);
        }

        status = gsl_linalg_cholesky_decomp1(K);
        if (status == 0) {
            fprintf(stderr, "Cholesky succeeded with fallback jitter %.2e (attempt %d)\n",
                    jitter, attempt);
            gsl_set_error_handler(old_handler);
            return 0;  // Success with jitter
        }

        jitter *= 10.0;  // Increase jitter for next attempt
    }

    // All attempts failed
    fprintf(stderr, "Cholesky decomposition failed after %d attempts (max jitter %.2e)\n",
            GP_JITTER_MAX_ATTEMPTS, jitter / 10.0);
    gsl_set_error_handler(old_handler);
    return status;
}

double rbf_kernel(double x, double xp, double sigma2, double ell) {
    double r = x - xp;
    return sigma2 * exp(-0.5 * r * r / (ell * ell));
}

double cross_covariance(double x, double y, double sigma2, double ell) {
    // K_fg(x, y) = integral_0^y k_ff(x, t) dt
    double sqrt_2 = sqrt(2.0);
    double sqrt_pi_2 = sqrt(M_PI / 2.0);

    double erf_1 = erf(x / (sqrt_2 * ell));
    double erf_2 = erf((x - y) / (sqrt_2 * ell));

    return sigma2 * ell * sqrt_pi_2 * (erf_1 - erf_2);
}

double integrated_rbf_kernel(double y, double yp, double sigma2, double ell) {
    // K_gg(y, y') = integral_0^y integral_0^y' k_ff(t, s) ds dt
    double sqrt_2 = sqrt(2.0);
    double sqrt_pi_2 = sqrt(M_PI / 2.0);
    double ell2 = ell * ell;

    // Error function terms
    double erf_y = erf(y / (sqrt_2 * ell));
    double erf_yp = erf(yp / (sqrt_2 * ell));
    double erf_diff = erf((y - yp) / (sqrt_2 * ell));

    // Exponential terms
    double exp_y = exp(-y*y / (2.0*ell2));
    double exp_yp = exp(-yp*yp / (2.0*ell2));
    double exp_diff = exp(-(y-yp)*(y-yp) / (2.0*ell2));

    // Full formula: two terms
    double term1 = sigma2 * ell * sqrt_pi_2 * (y * erf_y + yp * erf_yp - (y - yp) * erf_diff);
    double term2 = sigma2 * ell2 * (exp_y + exp_yp - exp_diff - 1.0);

    return term1 + term2;
}

// K_hh: Cov[f'(x), f'(x')] - derivative of derivative
double derivative_rbf_kernel(double x, double xp, double sigma2, double ell) {
    double r = x - xp;
    double r2 = r * r;
    double ell2 = ell * ell;
    return (sigma2 / ell2) * (1.0 - r2 / ell2) * exp(-0.5 * r2 / ell2);
}

// K_fh: Cov[f(x), f'(x')] - anti-symmetric!
double cross_covariance_f_h(double x, double xp, double sigma2, double ell) {
    double r = x - xp;
    double ell2 = ell * ell;
    return (sigma2 / ell2) * r * exp(-0.5 * r * r / ell2);
}

// K_gh: Cov[g(y), f'(x)] = integral_0^y K_fh(t, x) dt
double cross_covariance_g_h(double y, double x, double sigma2, double ell) {
    double ell2 = ell * ell;
    double exp_x = exp(-x*x / (2.0*ell2));
    double exp_diff = exp(-(y-x)*(y-x) / (2.0*ell2));
    return sigma2 * (exp_x - exp_diff);
}

// =============================================================================
// Matérn 5/2 Kernel Functions
// =============================================================================
// Base kernel: k(r) = σ² (1 + √5r/ℓ + 5r²/(3ℓ²)) exp(-√5r/ℓ)
// Matérn 5/2 is twice differentiable, making it suitable for derivative GPs.

#define SQRT5 2.2360679774997896964091736687747632

// K_ff for Matérn 5/2: k(x, x') = σ² (1 + √5r/ℓ + 5r²/(3ℓ²)) exp(-√5r/ℓ)
double matern52_kernel(double x, double xp, double sigma2, double ell) {
    double r = fabs(x - xp);
    double r_over_ell = r / ell;
    double sqrt5_r_ell = SQRT5 * r_over_ell;
    double r2_ell2 = r_over_ell * r_over_ell;

    return sigma2 * (1.0 + sqrt5_r_ell + (5.0/3.0) * r2_ell2) * exp(-sqrt5_r_ell);
}

// K_hh for Matérn 5/2: Cov[f'(x), f'(x')] = -d²k/dr² for stationary kernel
// Formula: (5σ²/3ℓ²) (1 + √5r/ℓ - 5r²/ℓ²) exp(-√5r/ℓ)
// Note: At r=0, this equals 5σ²/(3ℓ²)
double matern52_derivative_kernel(double x, double xp, double sigma2, double ell) {
    double r = fabs(x - xp);
    double ell2 = ell * ell;
    double r_over_ell = r / ell;
    double sqrt5_r_ell = SQRT5 * r_over_ell;
    double r2_ell2 = r_over_ell * r_over_ell;

    // (5σ²/3ℓ²) (1 + √5r/ℓ - 5r²/ℓ²) exp(-√5r/ℓ)
    double factor = (5.0 * sigma2) / (3.0 * ell2);
    double bracket = 1.0 + sqrt5_r_ell - 5.0 * r2_ell2;

    return factor * bracket * exp(-sqrt5_r_ell);
}

// K_fh for Matérn 5/2: Cov[f(x), f'(x')] = -dk/dx' (derivative w.r.t. second argument)
// Formula: (5σ²(x-x')/(3ℓ²)) (1 + √5r/ℓ) exp(-√5r/ℓ)
// Note: Anti-symmetric in argument swap: k_fh(x', x) = -k_fh(x, x')
double matern52_cross_covariance_f_h(double x, double xp, double sigma2, double ell) {
    double diff = x - xp;  // signed difference (NOT abs)
    double r = fabs(diff);
    double ell2 = ell * ell;
    double r_over_ell = r / ell;
    double sqrt5_r_ell = SQRT5 * r_over_ell;

    // (5σ²(x-x')/(3ℓ²)) (1 + √5r/ℓ) exp(-√5r/ℓ)
    double factor = (5.0 * sigma2 * diff) / (3.0 * ell2);
    double bracket = 1.0 + sqrt5_r_ell;

    return factor * bracket * exp(-sqrt5_r_ell);
}

// Helper: Antiderivative of Matérn 5/2 kernel for u >= 0
// F(u) = -σ²ℓ/(3√5) × (8 + 5√5u/ℓ + 5u²/ℓ²) × exp(-√5u/ℓ)
static double matern52_antideriv_positive(double u, double sigma2, double ell) {
    double ell2 = ell * ell;
    double ell_over_3sqrt5 = ell / (3.0 * SQRT5);
    double sqrt5_u_ell = SQRT5 * u / ell;
    double u2_ell2 = u * u / ell2;
    return -sigma2 * ell_over_3sqrt5 * (8.0 + 5.0 * sqrt5_u_ell + 5.0 * u2_ell2) * exp(-sqrt5_u_ell);
}

// Helper: Single integral of Matérn 5/2 kernel
// I(x, a, b) = ∫_a^b k_ff(x, t) dt
static double matern52_single_integral(double x, double a, double b, double sigma2, double ell) {
    // Using substitution u = t - x:
    // ∫_a^b k_ff(x,t) dt = ∫_{a-x}^{b-x} k_ff(0, u) du
    // Since k_ff only depends on |u| (symmetric kernel), we use:
    // For symmetric k(|u|): ∫_{a}^{b} k(|u|) du where a < 0 < b equals
    // [F(-a) - F(0)] + [F(b) - F(0)] = F(-a) + F(b) - 2*F(0)

    double ua = a - x;
    double ub = b - x;
    double result = 0.0;

    if (ua >= 0) {
        // Both ua and ub >= 0
        result = matern52_antideriv_positive(ub, sigma2, ell) - matern52_antideriv_positive(ua, sigma2, ell);
    } else if (ub <= 0) {
        // Both ua and ub <= 0, use symmetry: ∫_{ua}^{ub} k(|u|) du = ∫_{-ub}^{-ua} k(v) dv
        result = matern52_antideriv_positive(-ua, sigma2, ell) - matern52_antideriv_positive(-ub, sigma2, ell);
    } else {
        // ua < 0 < ub, split at 0
        // ∫_{ua}^{ub} k(|u|) du = [F(-ua) - F(0)] + [F(ub) - F(0)]
        double F0 = matern52_antideriv_positive(0.0, sigma2, ell);
        result = (matern52_antideriv_positive(-ua, sigma2, ell) - F0) + (matern52_antideriv_positive(ub, sigma2, ell) - F0);
    }

    return result;
}

// K_fg for Matérn 5/2: Cov[f(x), g(y)] = ∫_0^y k_ff(x, t) dt
double matern52_cross_covariance(double x, double y, double sigma2, double ell) {
    return matern52_single_integral(x, 0.0, y, sigma2, ell);
}

// Helper for double integral: ∫_0^y ∫_0^y' k_ff(s, t) ds dt
// This is the K_gg kernel for Matérn 5/2
// We compute this by first integrating over s (giving the single integral formula),
// then integrating over t.
//
// Actually, let's use a more direct approach via the formula:
// K_gg(y, y') = ∫_0^y ∫_0^y' k_ff(s, t) ds dt
// = ∫_0^y K_fg(t, y') dt  (where K_fg(t, y') = ∫_0^y' k_ff(t, s) ds)
//
// For numerical stability and clarity, we'll compute this using the closed-form
// double integral.

static double matern52_integrated_kernel_raw(double y, double yp, double sigma2, double ell) {
    double min0yp = fmin(0.0, yp);
    double minypmax0y = fmin(yp, fmax(0.0, y));

    if (y < 0.0) {
        return (1.0/15.0)*sigma2*(((-15*pow(ell, 2)*exp(sqrt(5)*y/ell) + 15*pow(ell, 2) + 7*sqrt(5)*ell*y + 7*sqrt(5)*ell*exp(sqrt(5)*y/ell)*min0yp - 7*sqrt(5)*ell*min0yp + 5*pow(y, 2) - 10*y*min0yp - 5*exp(sqrt(5)*y/ell)*pow(min0yp, 2) + 5*pow(min0yp, 2))*exp(sqrt(5)*min0yp/ell) + (15*pow(ell, 2)*exp(sqrt(5)*y/ell) - 15*pow(ell, 2) - 7*sqrt(5)*ell*y - 7*sqrt(5)*ell*exp(sqrt(5)*y/ell)*minypmax0y + 7*sqrt(5)*ell*minypmax0y - 5*pow(y, 2) + 10*y*minypmax0y + 5*exp(sqrt(5)*y/ell)*pow(minypmax0y, 2) - 5*pow(minypmax0y, 2))*exp(sqrt(5)*minypmax0y/ell))*exp(sqrt(5)*(2*y + yp + minypmax0y)/ell) + (-15*pow(ell, 2)*exp(sqrt(5)*y/ell) + 15*pow(ell, 2) + 7*sqrt(5)*ell*y*exp(sqrt(5)*y/ell) - 7*sqrt(5)*ell*yp*exp(sqrt(5)*y/ell) + 7*sqrt(5)*ell*yp - 5*pow(y, 2)*exp(sqrt(5)*y/ell) + 10*y*yp*exp(sqrt(5)*y/ell) - 5*pow(yp, 2)*exp(sqrt(5)*y/ell) + 5*pow(yp, 2))*exp(sqrt(5)*(3*y + minypmax0y)/ell) + (15*pow(ell, 2)*exp(sqrt(5)*y/ell) - 15*pow(ell, 2) - 7*sqrt(5)*ell*y*exp(sqrt(5)*y/ell) + 7*sqrt(5)*ell*exp(sqrt(5)*y/ell)*minypmax0y - 7*sqrt(5)*ell*minypmax0y + 5*pow(y, 2)*exp(sqrt(5)*y/ell) - 10*y*exp(sqrt(5)*y/ell)*minypmax0y + 5*exp(sqrt(5)*y/ell)*pow(minypmax0y, 2) - 5*pow(minypmax0y, 2))*exp(sqrt(5)*(3*y + yp)/ell))*exp(-sqrt(5)*(3*y + yp + minypmax0y)/ell);
    }

    return (1.0/15.0)*sigma2*(16*sqrt(5)*ell*(-min0yp + minypmax0y)*exp(sqrt(5)*(4*y + yp + min0yp + 2*minypmax0y)/ell) + (-15*pow(ell, 2)*exp(sqrt(5)*y/ell) - 7*sqrt(5)*ell*exp(sqrt(5)*y/ell)*min0yp + (15*pow(ell, 2) + 7*sqrt(5)*ell*y - 7*sqrt(5)*ell*min0yp + 5*pow(y, 2) - 10*y*min0yp + 5*pow(min0yp, 2))*exp(2*sqrt(5)*min0yp/ell) - 5*exp(sqrt(5)*y/ell)*pow(min0yp, 2))*exp(sqrt(5)*(3*y + yp + 2*minypmax0y)/ell) + (15*pow(ell, 2)*exp(sqrt(5)*y/ell) + 7*sqrt(5)*ell*exp(sqrt(5)*y/ell)*minypmax0y + (-15*pow(ell, 2) - 7*sqrt(5)*ell*y + 7*sqrt(5)*ell*minypmax0y - 5*pow(y, 2) + 10*y*minypmax0y - 5*pow(minypmax0y, 2))*exp(2*sqrt(5)*minypmax0y/ell) + 5*exp(sqrt(5)*y/ell)*pow(minypmax0y, 2))*exp(sqrt(5)*(3*y + yp + min0yp + minypmax0y)/ell) + (-15*pow(ell, 2)*exp(sqrt(5)*y/ell) + 15*pow(ell, 2) + 7*sqrt(5)*ell*y*exp(sqrt(5)*y/ell) - 7*sqrt(5)*ell*yp*exp(sqrt(5)*y/ell) + 7*sqrt(5)*ell*yp - 5*pow(y, 2)*exp(sqrt(5)*y/ell) + 10*y*yp*exp(sqrt(5)*y/ell) - 5*pow(yp, 2)*exp(sqrt(5)*y/ell) + 5*pow(yp, 2))*exp(sqrt(5)*(4*y + min0yp + 2*minypmax0y)/ell) + (15*pow(ell, 2)*exp(sqrt(5)*y/ell) - 15*pow(ell, 2) - 7*sqrt(5)*ell*y*exp(sqrt(5)*y/ell) + 7*sqrt(5)*ell*exp(sqrt(5)*y/ell)*minypmax0y - 7*sqrt(5)*ell*minypmax0y + 5*pow(y, 2)*exp(sqrt(5)*y/ell) - 10*y*exp(sqrt(5)*y/ell)*minypmax0y + 5*exp(sqrt(5)*y/ell)*pow(minypmax0y, 2) - 5*pow(minypmax0y, 2))*exp(sqrt(5)*(4*y + yp + min0yp + minypmax0y)/ell))*exp(-sqrt(5)*(4*y + yp + min0yp + 2*minypmax0y)/ell);
}

double matern52_integrated_kernel(double y, double yp, double sigma2, double ell) {
    // Use analytical form instead of quadrature
    return matern52_integrated_kernel_raw(y, yp, sigma2, ell);

    /* QUADRATURE VERSION (commented out for debugging)
    static const double gl_nodes[] = {
        -0.9931285991850949, -0.9639719272779138, -0.9122344282513259,
        -0.8391169718222188, -0.7463319064601508, -0.6360536807265150,
        -0.5108670019508271, -0.3737060887154196, -0.2277858511416451,
        -0.0765265211334973,  0.0765265211334973,  0.2277858511416451,
         0.3737060887154196,  0.5108670019508271,  0.6360536807265150,
         0.7463319064601508,  0.8391169718222188,  0.9122344282513259,
         0.9639719272779138,  0.9931285991850949
    };
    static const double gl_weights[] = {
        0.0176140071391521, 0.0406014298003869, 0.0626720483341091,
        0.0832767415767048, 0.1019301198172404, 0.1181945319615184,
        0.1316886384491766, 0.1420961093183820, 0.1491729864726037,
        0.1527533871307258, 0.1527533871307258, 0.1491729864726037,
        0.1420961093183820, 0.1316886384491766, 0.1181945319615184,
        0.1019301198172404, 0.0832767415767048, 0.0626720483341091,
        0.0406014298003869, 0.0176140071391521
    };

    double result = 0.0;
    double half_y = 0.5 * y;
    double half_yp = 0.5 * yp;

    for (int i = 0; i < 20; i++) {
        double s = half_y * (1.0 + gl_nodes[i]);
        for (int j = 0; j < 20; j++) {
            double t = half_yp * (1.0 + gl_nodes[j]);
            result += gl_weights[i] * gl_weights[j] * matern52_kernel(s, t, sigma2, ell);
        }
    }

    result *= half_y * half_yp;
    return result;
    */
}

// Helper: Antiderivative of u*(1 + √5u/ℓ)*exp(-√5u/ℓ) for u >= 0
// This is used for computing K_gh
static double matern52_kfh_antideriv_positive(double u, double sigma2, double ell) {
    double sqrt5_ell = SQRT5 / ell;
    double ell2 = ell * ell;
    double coeff = 5.0 * sigma2 / (3.0 * ell2);
    double alpha = sqrt5_ell;

    if (u < 1e-12) {
        // At u=0: F+(0) = coeff × [-(ℓ²/5) - (√5/ℓ)(2ℓ³/(5√5))] = -σ²
        return -sigma2;
    }

    double exp_term = exp(-alpha * u);
    // ∫ u exp(-αu) du = -(u/α + 1/α²) exp(-αu)
    double int_u = -(u/alpha + 1.0/(alpha*alpha)) * exp_term;
    // ∫ u² exp(-αu) du = -(u²/α + 2u/α² + 2/α³) exp(-αu)
    double int_u2 = -(u*u/alpha + 2.0*u/(alpha*alpha) + 2.0/(alpha*alpha*alpha)) * exp_term;
    // F+(u) = coeff × [int_u + (√5/ℓ) int_u2]
    return coeff * (int_u + sqrt5_ell * int_u2);
}

// K_gh for Matérn 5/2: Cov[g(y), h(x)] = ∫_0^y k_fh(t, x) dt
double matern52_cross_covariance_g_h(double y, double x, double sigma2, double ell) {
    // Let u = t - x, then:
    // ∫_{-x}^{y-x} (5σ²u/(3ℓ²)) (1 + √5|u|/ℓ) exp(-√5|u|/ℓ) du
    // The integrand is odd in u.

    double ua = -x;
    double ub = y - x;
    double result = 0.0;

    if (ua >= 0) {
        // Both limits positive
        result = matern52_kfh_antideriv_positive(ub, sigma2, ell) - matern52_kfh_antideriv_positive(ua, sigma2, ell);
    } else if (ub <= 0) {
        // Both limits negative: use odd function property
        // For odd f: ∫_{ua}^{ub} f(u) du = -∫_{-ub}^{-ua} f(v) dv = F+(-ub) - F+(-ua)
        result = matern52_kfh_antideriv_positive(-ub, sigma2, ell) - matern52_kfh_antideriv_positive(-ua, sigma2, ell);
    } else {
        // ua < 0 < ub: split at 0
        // ∫_{ua}^{0} f(u) du: for odd f, this equals -(∫_{0}^{-ua} f(v) dv) = -[F+(-ua) - F+(0)]
        // ∫_{0}^{ub} f(u) du = F+(ub) - F+(0)
        double F0 = matern52_kfh_antideriv_positive(0.0, sigma2, ell);
        double neg_part = -(matern52_kfh_antideriv_positive(-ua, sigma2, ell) - F0);
        double pos_part = matern52_kfh_antideriv_positive(ub, sigma2, ell) - F0;
        result = neg_part + pos_part;
    }

    return result;
}

// =============================================================================
// Matérn 3/2 Kernel Functions
// =============================================================================
// Base kernel: k(r) = σ² (1 + √3r/ℓ) exp(-√3r/ℓ)
// Matérn 3/2 is once mean-square differentiable, so derivative observations (h) ARE supported.

#define SQRT3 1.7320508075688772935274463415058724

// K_ff for Matérn 3/2: k(x, x') = σ² (1 + √3r/ℓ) exp(-√3r/ℓ)
double matern32_kernel(double x, double xp, double sigma2, double ell) {
    double r = fabs(x - xp);
    double sqrt3_r_ell = SQRT3 * r / ell;
    return sigma2 * (1.0 + sqrt3_r_ell) * exp(-sqrt3_r_ell);
}

// Helper: Antiderivative of Matérn 3/2 kernel for u >= 0
// F(u) = -σ²(2ℓ/√3 + u) exp(-√3u/ℓ)
static double matern32_antideriv_positive(double u, double sigma2, double ell) {
    double sqrt3_u_ell = SQRT3 * u / ell;
    double two_ell_sqrt3 = 2.0 * ell / SQRT3;
    return -sigma2 * (two_ell_sqrt3 + u) * exp(-sqrt3_u_ell);
}

// Helper: Single integral of Matérn 3/2 kernel
// I(x, a, b) = ∫_a^b k_ff(x, t) dt
static double matern32_single_integral(double x, double a, double b, double sigma2, double ell) {
    double ua = a - x;
    double ub = b - x;
    double result = 0.0;

    if (ua >= 0) {
        // Both ua and ub >= 0
        result = matern32_antideriv_positive(ub, sigma2, ell) - matern32_antideriv_positive(ua, sigma2, ell);
    } else if (ub <= 0) {
        // Both ua and ub <= 0, use symmetry
        result = matern32_antideriv_positive(-ua, sigma2, ell) - matern32_antideriv_positive(-ub, sigma2, ell);
    } else {
        // ua < 0 < ub, split at 0
        double F0 = matern32_antideriv_positive(0.0, sigma2, ell);
        result = (matern32_antideriv_positive(-ua, sigma2, ell) - F0) + (matern32_antideriv_positive(ub, sigma2, ell) - F0);
    }

    return result;
}

// K_fg for Matérn 3/2: Cov[f(x), g(y)] = ∫_0^y k_ff(x, t) dt
double matern32_cross_covariance(double x, double y, double sigma2, double ell) {
    return matern32_single_integral(x, 0.0, y, sigma2, ell);
}

// K_gg for Matérn 3/2: double integral using Gauss-Legendre quadrature
static double matern32_integrated_kernel_raw(double y, double yp, double sigma2, double ell) {
    double min0yp = fmin(0.0, yp);
    double minypmax0y = fmin(yp, fmax(0.0, y));

    if (y < 0.0) {
        return (1.0/3.0)*ell*sigma2*((((-3*ell*exp(sqrt(3)*y/ell) + 3*ell + sqrt(3)*y + sqrt(3)*exp(sqrt(3)*y/ell)*min0yp - sqrt(3)*min0yp)*exp(sqrt(3)*min0yp/ell) + (3*ell*exp(sqrt(3)*y/ell) - 3*ell - sqrt(3)*y - sqrt(3)*exp(sqrt(3)*y/ell)*minypmax0y + sqrt(3)*minypmax0y)*exp(sqrt(3)*minypmax0y/ell))*exp(sqrt(3)*(y + yp + minypmax0y)/ell) + (-3*ell*exp(sqrt(3)*y/ell) + 3*ell + sqrt(3)*y*exp(sqrt(3)*y/ell) - sqrt(3)*yp*exp(sqrt(3)*y/ell) + sqrt(3)*yp)*exp(sqrt(3)*(2*y + minypmax0y)/ell) + (3*ell*exp(sqrt(3)*y/ell) - 3*ell - sqrt(3)*y*exp(sqrt(3)*y/ell) + sqrt(3)*exp(sqrt(3)*y/ell)*minypmax0y - sqrt(3)*minypmax0y)*exp(sqrt(3)*(2*y + yp)/ell))*exp(-sqrt(3)*(2*y + yp + minypmax0y)/ell));
    }

    return (1.0/3.0)*ell*sigma2*(4*sqrt(3)*(-min0yp + minypmax0y)*exp(sqrt(3)*(2*y + yp + min0yp + 2*minypmax0y)/ell) + (-3*ell*exp(sqrt(3)*y/ell) + (3*ell + sqrt(3)*y - sqrt(3)*min0yp)*exp(2*sqrt(3)*min0yp/ell) - sqrt(3)*exp(sqrt(3)*y/ell)*min0yp)*exp(sqrt(3)*(y + yp + 2*minypmax0y)/ell) + (3*ell*exp(sqrt(3)*y/ell) + (-3*ell - sqrt(3)*y + sqrt(3)*minypmax0y)*exp(2*sqrt(3)*minypmax0y/ell) + sqrt(3)*exp(sqrt(3)*y/ell)*minypmax0y)*exp(sqrt(3)*(y + yp + min0yp + minypmax0y)/ell) + (-3*ell*exp(sqrt(3)*y/ell) + 3*ell + sqrt(3)*y*exp(sqrt(3)*y/ell) - sqrt(3)*yp*exp(sqrt(3)*y/ell) + sqrt(3)*yp)*exp(sqrt(3)*(2*y + min0yp + 2*minypmax0y)/ell) + (3*ell*exp(sqrt(3)*y/ell) - 3*ell - sqrt(3)*y*exp(sqrt(3)*y/ell) + sqrt(3)*exp(sqrt(3)*y/ell)*minypmax0y - sqrt(3)*minypmax0y)*exp(sqrt(3)*(2*y + yp + min0yp + minypmax0y)/ell))*exp(-sqrt(3)*(2*y + yp + min0yp + 2*minypmax0y)/ell);
}

double matern32_integrated_kernel(double y, double yp, double sigma2, double ell) {
    // Use analytical form instead of quadrature
    return matern32_integrated_kernel_raw(y, yp, sigma2, ell);

    /* QUADRATURE VERSION (commented out for debugging)
    static const double gl_nodes[] = {
        -0.9931285991850949, -0.9639719272779138, -0.9122344282513259,
        -0.8391169718222188, -0.7463319064601508, -0.6360536807265150,
        -0.5108670019508271, -0.3737060887154196, -0.2277858511416451,
        -0.0765265211334973,  0.0765265211334973,  0.2277858511416451,
         0.3737060887154196,  0.5108670019508271,  0.6360536807265150,
         0.7463319064601508,  0.8391169718222188,  0.9122344282513259,
         0.9639719272779138,  0.9931285991850949
    };
    static const double gl_weights[] = {
        0.0176140071391521, 0.0406014298003869, 0.0626720483341091,
        0.0832767415767048, 0.1019301198172404, 0.1181945319615184,
        0.1316886384491766, 0.1420961093183820, 0.1491729864726037,
        0.1527533871307258, 0.1527533871307258, 0.1491729864726037,
        0.1420961093183820, 0.1316886384491766, 0.1181945319615184,
        0.1019301198172404, 0.0832767415767048, 0.0626720483341091,
        0.0406014298003869, 0.0176140071391521
    };

    double result = 0.0;
    double half_y = 0.5 * y;
    double half_yp = 0.5 * yp;

    for (int i = 0; i < 20; i++) {
        double s = half_y * (1.0 + gl_nodes[i]);
        for (int j = 0; j < 20; j++) {
            double t = half_yp * (1.0 + gl_nodes[j]);
            result += gl_weights[i] * gl_weights[j] * matern32_kernel(s, t, sigma2, ell);
        }
    }

    result *= half_y * half_yp;
    return result;
    */
}

// K_hh for Matérn 3/2: Cov[f'(x), f'(x')] = -d²k/dr² for stationary kernel
// Formula (from sympy): (3σ²/ℓ²)(1 - √3r/ℓ) exp(-√3r/ℓ)
// At r=0: k_hh(0) = 3σ²/ℓ² (finite derivative variance)
double matern32_derivative_kernel(double x, double xp, double sigma2, double ell) {
    double r = fabs(x - xp);
    double sqrt3_r_ell = SQRT3 * r / ell;
    return (3.0 * sigma2 / (ell * ell)) * (1.0 - sqrt3_r_ell) * exp(-sqrt3_r_ell);
}

// K_fh for Matérn 3/2: Cov[f(x), f'(x')] = -dk/dx'
// Formula (from sympy): (3σ²/ℓ²)(x - x') exp(-√3|x-x'|/ℓ)
// Anti-symmetric: k_fh(x', x) = -k_fh(x, x')
double matern32_cross_covariance_f_h(double x, double xp, double sigma2, double ell) {
    double diff = x - xp;  // signed difference (NOT abs)
    double r = fabs(diff);
    double sqrt3_r_ell = SQRT3 * r / ell;
    return (3.0 * sigma2 / (ell * ell)) * diff * exp(-sqrt3_r_ell);
}

// Helper: Antiderivative of (3σ²/ℓ²) u exp(-√3u/ℓ) for u >= 0
// F+(u) = -σ²(1 + √3u/ℓ) exp(-√3u/ℓ)
// Verified: d/du F+(u) = (3σ²/ℓ²) u exp(-√3u/ℓ) ✓
static double matern32_kfh_antideriv_positive(double u, double sigma2, double ell) {
    double sqrt3_u_ell = SQRT3 * u / ell;
    return -sigma2 * (1.0 + sqrt3_u_ell) * exp(-sqrt3_u_ell);
}

// K_gh for Matérn 3/2: Cov[g(y), h(x)] = ∫_0^y k_fh(t, x) dt
// Uses substitution u = t - x; integrand is odd in u.
double matern32_cross_covariance_g_h(double y, double x, double sigma2, double ell) {
    double ua = -x;
    double ub = y - x;
    double result = 0.0;

    if (ua >= 0) {
        // Both limits positive
        result = matern32_kfh_antideriv_positive(ub, sigma2, ell) - matern32_kfh_antideriv_positive(ua, sigma2, ell);
    } else if (ub <= 0) {
        // Both limits negative: use odd function property
        result = matern32_kfh_antideriv_positive(-ub, sigma2, ell) - matern32_kfh_antideriv_positive(-ua, sigma2, ell);
    } else {
        // ua < 0 < ub: split at 0
        double F0 = matern32_kfh_antideriv_positive(0.0, sigma2, ell);
        double neg_part = -(matern32_kfh_antideriv_positive(-ua, sigma2, ell) - F0);
        double pos_part = matern32_kfh_antideriv_positive(ub, sigma2, ell) - F0;
        result = neg_part + pos_part;
    }

    return result;
}

// =============================================================================
// Periodic Kernel Functions
// =============================================================================
// Base kernel: k(r) = σ² exp(-2 sin²(πr/p) / ℓ²)
// where r = |x - x'|, p = period, ℓ = length scale
//
// NOTE: Periodic kernels do NOT support integral observations (g).
//       The integrals would require numerical quadrature (Phase 2).

// K_ff for Periodic: k(x, x') = σ² exp(-2 sin²(πr/p) / ℓ²)
double periodic_kernel(double x, double xp, double sigma2, double ell, double period) {
    double r = fabs(x - xp);
    double u = M_PI * r / period;  // u = πr/p
    double sin_u = sin(u);
    double sin2_u = sin_u * sin_u;
    double ell2 = ell * ell;

    // exp_term = exp(-2*sin²(u)/ℓ²)
    double exp_term = exp(-2.0 * sin2_u / ell2);

    return sigma2 * exp_term;
}

// K_hh for Periodic: Cov[f'(x), f'(x')] = d²k/dxdx'
//
// Derivation using chain rule for k = σ² exp(f) where f = -2sin²(u)/ℓ²:
//   d²k/dxdx' = σ² exp(f) × [d²f/dxdx' - (df/dx)²]
//
// With u = πr/p and r = x - x':
//   df/dr = -4sin(u)cos(u)π/(pℓ²) = -2sin(2u)π/(pℓ²)
//   d²f/dr² = -4π²cos(2u)/(p²ℓ²)
//   d²f/dxdx' = -d²f/dr² = 4π²cos(2u)/(p²ℓ²)
//
// So: K_hh = σ² exp(f) × [4π²cos(2u)/(p²ℓ²) - 4sin²(2u)π²/(p²ℓ⁴)]
//          = 4π²σ² exp(f) × [ℓ²cos(2u) - sin²(2u)] / (p²ℓ⁴)
//
// At r=0: u=0, cos(0)=1, sin(0)=0 → K_hh(0) = 4π²σ²ℓ²/(p²ℓ⁴) = 4π²σ²/(p²ℓ²) ✓
double periodic_derivative_kernel(double x, double xp, double sigma2, double ell, double period) {
    double r = fabs(x - xp);
    double u = M_PI * r / period;
    double sin_u = sin(u);
    double sin2_u = sin_u * sin_u;
    double ell2 = ell * ell;
    double ell4 = ell2 * ell2;
    double p2 = period * period;

    double exp_term = exp(-2.0 * sin2_u / ell2);

    // Use double-angle formulas: cos(2u), sin(2u)
    double cos_2u = cos(2.0 * u);
    double sin_2u = sin(2.0 * u);
    double sin2_2u = sin_2u * sin_2u;

    // K_hh = 4π²σ² exp_term × (ℓ²cos(2u) - sin²(2u)) / (ℓ⁴p²)
    double numerator = ell2 * cos_2u - sin2_2u;
    double coeff = (4.0 * M_PI * M_PI * sigma2) / (ell4 * p2);

    return coeff * exp_term * numerator;
}

// K_fh for Periodic: Cov[f(x), f'(x')] - anti-symmetric
// Formula: k_fh = +4πσ²*sin(u)*cos(u)*exp_term / (ℓ²*p)
//                = +2πσ²*sin(2u)*exp_term / (ℓ²*p)
// where u = πr/p, but we need the SIGNED difference for anti-symmetry
//
// Anti-symmetry: k_fh(x, x') = -k_fh(x', x)
// So we use diff = x - xp (signed) instead of r = |x - xp|
double periodic_cross_covariance_f_h(double x, double xp, double sigma2, double ell, double period) {
    double diff = x - xp;  // SIGNED difference for anti-symmetry
    double r = fabs(diff);
    double u = M_PI * r / period;
    double sin_u = sin(u);
    double cos_u = cos(u);
    double sin2_u = sin_u * sin_u;
    double ell2 = ell * ell;

    double exp_term = exp(-2.0 * sin2_u / ell2);

    // k_fh = +4π*σ²*sign(diff)*sin(u)*cos(u)*exp_term / (ℓ²*p)
    // Use sign(diff) to preserve anti-symmetry
    double sign_diff = (diff >= 0.0) ? 1.0 : -1.0;
    double coeff = (4.0 * M_PI * sigma2) / (ell2 * period);

    return coeff * sign_diff * sin_u * cos_u * exp_term;
}

// =============================================================================
// Locally-Periodic Kernel Functions
// =============================================================================
// Base kernel: k(r) = σ² exp(-2 sin²(πr/p) / ℓ_p² - r²/(2ℓ_se²))
// This is a periodic kernel modulated by an SE decay envelope
//
// Parameters:
//   - ell: SE decay length scale (ℓ_se)
//   - ell_p: period length scale (ℓ_p)
//   - p: period (p)

// K_ff for Locally-Periodic
double locally_periodic_kernel(double x, double xp, double sigma2, double ell, double ell_p, double period) {
    double r = fabs(x - xp);
    double u = M_PI * r / period;
    double sin_u = sin(u);
    double sin2_u = sin_u * sin_u;
    double r2 = r * r;
    double ell2 = ell * ell;        // ℓ² (SE decay)
    double ell_p2 = ell_p * ell_p;  // ℓ_p² (periodic)

    // exp_term = exp(-2*sin²(u)/ℓ_p² - r²/(2ℓ²))
    double exp_term = exp(-2.0 * sin2_u / ell_p2 - r2 / (2.0 * ell2));

    return sigma2 * exp_term;
}

// K_hh for Locally-Periodic
// This requires applying the product rule to:
// k(r) = k_periodic(r) × k_SE(r)
// where k_periodic = exp(-2sin²(πr/p)/ℓ_p²) and k_SE = exp(-r²/(2ℓ_se²))
//
// For stationary kernels: k_hh = -d²k/dr²
// Using product rule: (uv)'' = u''v + 2u'v' + uv''
//
// Let f = exp(-2sin²(u)/ℓ_p²), g = exp(-r²/(2ℓ_se²))
// f' = -f × (4sin(u)cos(u)π/p)/ℓ_p² = -f × (2sin(2u)π/p)/ℓ_p²
// K_hh for Locally-Periodic kernel: Cov[f'(x), f'(x')] = d²k/dxdx'
//
// Using the chain rule for k = σ² exp(f) where f = -2sin²(πr/p)/ℓ_p² - r²/(2ℓ²):
//   d²k/dxdx' = σ² exp(f) × [d²f/dxdx' - (df/dx)²]
//
// where:
//   df/dx = -2sin(2u)π/(pℓ_p²) - r/ℓ²
//   d²f/dxdx' = 4π²cos(2u)/(p²ℓ_p²) + 1/ℓ²
//
// Note: r = x - x' is SIGNED (not absolute value) for correct derivatives.
double locally_periodic_derivative_kernel(double x, double xp, double sigma2, double ell, double ell_p, double period) {
    // ell = SE decay length scale, ell_p = periodic length scale
    double r = x - xp;  // SIGNED difference for correct derivative
    double u = M_PI * r / period;
    double sin_u = sin(u);
    double sin_2u = sin(2.0 * u);
    double cos_2u = cos(2.0 * u);
    double ell2 = ell * ell;
    double ell_p2 = ell_p * ell_p;
    double p2 = period * period;

    // f = -2sin²(u)/ℓ_p² - r²/(2ℓ²)
    double f = -2.0 * sin_u * sin_u / ell_p2 - r * r / (2.0 * ell2);
    double exp_f = exp(f);

    // df/dx = -2sin(2u)π/(pℓ_p²) - r/ℓ²
    double df_dx = -2.0 * sin_2u * M_PI / (period * ell_p2) - r / ell2;

    // d²f/dxdx' = 4π²cos(2u)/(p²ℓ_p²) + 1/ℓ²
    double d2f_dxdxp = 4.0 * M_PI * M_PI * cos_2u / (p2 * ell_p2) + 1.0 / ell2;

    // K_hh = σ² exp(f) × [d²f/dxdx' - (df/dx)²]
    return sigma2 * exp_f * (d2f_dxdxp - df_dx * df_dx);
}

// K_fh for Locally-Periodic - anti-symmetric
double locally_periodic_cross_covariance_f_h(double x, double xp, double sigma2, double ell, double ell_p, double period) {
    // ell = SE decay length scale, ell_p = periodic length scale
    double diff = x - xp;  // SIGNED difference
    double r = fabs(diff);
    double u = M_PI * r / period;
    double sin_u = sin(u);
    double cos_u = cos(u);
    double sin2_u = sin_u * sin_u;
    double r2 = r * r;
    double ell2 = ell * ell;        // ℓ² (SE decay)
    double ell_p2 = ell_p * ell_p;  // ℓ_p² (periodic)

    double exp_term = exp(-2.0 * sin2_u / ell_p2 - r2 / (2.0 * ell2));

    // k_fh = d(k)/dr where k = k_per × k_SE
    // Using chain rule: (fg)' = f'g + fg'
    //
    // k_per' = -k_per × (4sin(u)cos(u)π/p)/ℓ_p²
    // k_SE' = -k_SE × r/ℓ²
    //
    // So: k_fh = +σ² × sign(diff) × exp_term × [4sin(u)cos(u)π/(pℓ_p²) + r/ℓ²]
    //
    // But we need to account for sign for anti-symmetry
    double sign_diff = (diff >= 0.0) ? 1.0 : -1.0;

    double periodic_part = (4.0 * sin_u * cos_u * M_PI) / (period * ell_p2);
    double se_part = r / ell2;

    return sigma2 * sign_diff * exp_term * (periodic_part + se_part);
}

// =============================================================================
// Second-derivative (u = f'') kernel functions — Locally Periodic
// No K_gu: locally-periodic does not support integral observations.
// Formulas derived from k(t) = σ² exp(-2sin²(πt/p)/ℓ_p²) exp(-t²/(2ℓ²))
// via the Leibniz rule: all four derivatives computed symbolically.
// =============================================================================

/* K_fu for locally-periodic: k''(t) = ∂²k/∂x'²
 * = σ²(π²ℓ⁴(-4ℓ_p²cos(2πt/p) - 2cos(4πt/p) + 2)
 *      + 4πℓ²ℓ_p²pt·sin(2πt/p)
 *      + ℓ_p⁴p²(t² - ℓ²)) / (ℓ⁴ℓ_p⁴p²) × exp_term */
static double locally_periodic_cross_covariance_f_u(double x, double xp, double sigma2, double ell, double ell_p, double period) {
    double t   = x - xp;
    double ell2  = ell * ell;
    double ell4  = ell2 * ell2;
    double ellp2 = ell_p * ell_p;
    double ellp4 = ellp2 * ellp2;
    double p2    = period * period;
    double pi_t_p = M_PI * t / period;
    double sin_t  = sin(pi_t_p);
    double exp_term = exp(-2.0 * sin_t * sin_t / ellp2 - 0.5 * t * t / ell2);

    double bracket = M_PI * M_PI * ell4 * (-4.0 * ellp2 * cos(2.0 * pi_t_p)
                                            - 2.0 * cos(4.0 * pi_t_p) + 2.0)
                   + 4.0 * M_PI * ell2 * ellp2 * period * t * sin(2.0 * pi_t_p)
                   + ellp4 * p2 * (t * t - ell2);
    return sigma2 * bracket * exp_term / (ell4 * ellp4 * p2);
}

/* K_uu for locally-periodic: k''''(t) = ∂⁴k/∂t⁴ */
static double locally_periodic_second_derivative_kernel(double x, double xp, double sigma2, double ell, double ell_p, double period) {
    double t     = x - xp;
    double ell2  = ell * ell;
    double ell4  = ell2 * ell2;
    double ell6  = ell4 * ell2;
    double ell8  = ell4 * ell4;
    double ellp2 = ell_p * ell_p;
    double ellp4 = ellp2 * ellp2;
    double ellp6 = ellp4 * ellp2;
    double ellp8 = ellp4 * ellp4;
    double p2    = period * period;
    double p4    = p2 * p2;
    double pi_t_p = M_PI * t / period;
    double sin_t  = sin(pi_t_p);
    double cos_t  = cos(pi_t_p);
    double sin2   = sin_t * sin_t;
    double sin4   = sin2 * sin2;
    double cos4   = cos_t * cos_t * cos_t * cos_t;
    double exp_term = exp(-2.0 * sin2 / ellp2 - 0.5 * t * t / ell2);

    double term1 = 16.0 * M_PI * M_PI * M_PI * M_PI * ell8
                   * (ellp6 * cos(2.0 * pi_t_p)
                      - ellp4 * (-28.0 * sin4 + 28.0 * sin2 - 3.0)
                      - 1.5 * ellp2 * (cos(2.0 * pi_t_p) - cos(6.0 * pi_t_p))
                      + 16.0 * sin4 * cos4);
    double term2 = -64.0 * M_PI * M_PI * M_PI * ell6 * ellp2 * period * t
                   * (ellp4 + 3.0 * ellp2 * cos(2.0 * pi_t_p)
                      + 0.5 * cos(4.0 * pi_t_p) - 0.5)
                   * sin_t * cos_t;
    double term3 = 24.0 * M_PI * M_PI * ell4 * ellp4 * p2 * (ell2 - t * t)
                   * (ellp2 * cos(2.0 * pi_t_p) + 0.5 * cos(4.0 * pi_t_p) - 0.5);
    double term4 = -8.0 * M_PI * ell2 * ellp6 * p2 * period * t
                   * (3.0 * ell2 - t * t) * sin(2.0 * pi_t_p);
    double term5 = ellp8 * p4 * (3.0 * ell4 - 6.0 * ell2 * t * t + t * t * t * t);

    return sigma2 * (term1 + term2 + term3 + term4 + term5)
           * exp_term / (ell8 * ellp8 * p4);
}

/* K_hu for locally-periodic: k'''(t) = ∂³k/∂t³  (antisymmetric in t) */
static double locally_periodic_cross_covariance_h_u(double x, double xp, double sigma2, double ell, double ell_p, double period) {
    double t     = x - xp;
    double ell2  = ell * ell;
    double ell4  = ell2 * ell2;
    double ell6  = ell4 * ell2;
    double ellp2 = ell_p * ell_p;
    double ellp4 = ellp2 * ellp2;
    double ellp6 = ellp4 * ellp2;
    double p2    = period * period;
    double p3    = p2 * period;
    double pi_t_p = M_PI * t / period;
    double sin_t  = sin(pi_t_p);
    double cos_t  = cos(pi_t_p);
    double sin2   = sin_t * sin_t;
    double exp_term = exp(-2.0 * sin2 / ellp2 - 0.5 * t * t / ell2);

    double term1 = 16.0 * M_PI * M_PI * M_PI * ell6
                   * (ellp4 + 3.0 * ellp2 * cos(2.0 * pi_t_p)
                      + 0.5 * cos(4.0 * pi_t_p) - 0.5)
                   * sin_t * cos_t;
    double term2 = -12.0 * M_PI * M_PI * ell4 * ellp2 * period * t
                   * (-ellp2 * cos(2.0 * pi_t_p) - 0.5 * cos(4.0 * pi_t_p) + 0.5);
    double term3 = 6.0 * M_PI * ell2 * ellp4 * p2 * (ell2 - t * t) * sin(2.0 * pi_t_p);
    double term4 = ellp6 * p3 * t * (3.0 * ell2 - t * t);

    return sigma2 * (term1 + term2 + term3 + term4)
           * exp_term / (ell6 * ellp6 * p3);
}

// =============================================================================
// Kernel Dispatch Functions
// =============================================================================

static inline double compute_k_ff(double x, double xp, double sigma2, double ell, double ell_p, double period, KernelType kernel) {
    switch (kernel) {
        case KERNEL_MATERN52: return matern52_kernel(x, xp, sigma2, ell);
        case KERNEL_MATERN32: return matern32_kernel(x, xp, sigma2, ell);
        case KERNEL_PERIODIC: return periodic_kernel(x, xp, sigma2, ell_p, period);
        case KERNEL_LOCALLY_PERIODIC:
            // ell = SE decay length scale, ell_p = periodic length scale
            return locally_periodic_kernel(x, xp, sigma2, ell, ell_p, period);
        default: return rbf_kernel(x, xp, sigma2, ell);
    }
}

static inline double compute_k_gg(double y, double yp, double sigma2, double ell, double ell_p, double period, KernelType kernel) {
    (void)ell_p;  // Unused for non-periodic kernels (periodic doesn't support g)
    (void)period;
    // Periodic kernels do NOT support integral observations
    if (kernel == KERNEL_PERIODIC || kernel == KERNEL_LOCALLY_PERIODIC) {
        fprintf(stderr, "ERROR: Integral observations (g) are not supported for periodic kernels.\n");
        fprintf(stderr, "       The integrals of periodic kernels require numerical quadrature.\n");
        return 0.0;  // Should not be called, but return 0 for safety
    }

    switch (kernel) {
        case KERNEL_MATERN52: return matern52_integrated_kernel(y, yp, sigma2, ell);
        case KERNEL_MATERN32: return matern32_integrated_kernel(y, yp, sigma2, ell);
        default: return integrated_rbf_kernel(y, yp, sigma2, ell);
    }
}

static inline double compute_k_hh(double x, double xp, double sigma2, double ell, double ell_p, double period, KernelType kernel) {
    switch (kernel) {
        case KERNEL_MATERN52: return matern52_derivative_kernel(x, xp, sigma2, ell);
        case KERNEL_MATERN32: return matern32_derivative_kernel(x, xp, sigma2, ell);
        case KERNEL_PERIODIC: return periodic_derivative_kernel(x, xp, sigma2, ell_p, period);
        case KERNEL_LOCALLY_PERIODIC:
            // ell = SE decay length scale, ell_p = periodic length scale
            return locally_periodic_derivative_kernel(x, xp, sigma2, ell, ell_p, period);
        default: return derivative_rbf_kernel(x, xp, sigma2, ell);
    }
}

static inline double compute_k_fg(double x, double y, double sigma2, double ell, double ell_p, double period, KernelType kernel) {
    (void)ell_p;  // Unused for non-periodic kernels (periodic doesn't support g)
    (void)period;
    // Periodic kernels do NOT support integral observations
    if (kernel == KERNEL_PERIODIC || kernel == KERNEL_LOCALLY_PERIODIC) {
        fprintf(stderr, "ERROR: Integral observations (g) are not supported for periodic kernels.\n");
        fprintf(stderr, "       The integrals of periodic kernels require numerical quadrature (Phase 2).\n");
        return 0.0;  // Should not be called, but return 0 for safety
    }

    switch (kernel) {
        case KERNEL_MATERN52: return matern52_cross_covariance(x, y, sigma2, ell);
        case KERNEL_MATERN32: return matern32_cross_covariance(x, y, sigma2, ell);
        default: return cross_covariance(x, y, sigma2, ell);
    }
}

static inline double compute_k_fh(double x, double xp, double sigma2, double ell, double ell_p, double period, KernelType kernel) {
    switch (kernel) {
        case KERNEL_MATERN52: return matern52_cross_covariance_f_h(x, xp, sigma2, ell);
        case KERNEL_MATERN32: return matern32_cross_covariance_f_h(x, xp, sigma2, ell);
        case KERNEL_PERIODIC: return periodic_cross_covariance_f_h(x, xp, sigma2, ell_p, period);
        case KERNEL_LOCALLY_PERIODIC:
            // ell = SE decay length scale, ell_p = periodic length scale
            return locally_periodic_cross_covariance_f_h(x, xp, sigma2, ell, ell_p, period);
        default: return cross_covariance_f_h(x, xp, sigma2, ell);
    }
}

static inline double compute_k_gh(double y, double x, double sigma2, double ell, double ell_p, double period, KernelType kernel) {
    (void)ell_p;  // Unused for non-periodic kernels (periodic doesn't support g)
    (void)period;
    // Periodic kernels do NOT support integral observations
    if (kernel == KERNEL_PERIODIC || kernel == KERNEL_LOCALLY_PERIODIC) {
        fprintf(stderr, "ERROR: Integral observations (g) are not supported for periodic kernels.\n");
        fprintf(stderr, "       The integrals of periodic kernels require numerical quadrature (Phase 2).\n");
        return 0.0;  // Should not be called, but return 0 for safety
    }

    switch (kernel) {
        case KERNEL_MATERN52: return matern52_cross_covariance_g_h(y, x, sigma2, ell);
        case KERNEL_MATERN32: return matern32_cross_covariance_g_h(y, x, sigma2, ell);
        default: return cross_covariance_g_h(y, x, sigma2, ell);
    }
}

// =============================================================================
// Second-derivative (u = f'') kernel dispatch helpers (RBF only)
// =============================================================================

/* K_uu(x, x') = Cov[f''(x), f''(x')] = (σ²/ℓ⁴)(3 - 6ρ² + ρ⁴) exp(-ρ²/2), ρ = (x-x')/ℓ */
double rbf_second_derivative_kernel(double x, double xp, double sigma2, double ell) {
    double r = x - xp;
    double r2 = r * r;
    double ell2 = ell * ell;
    double rho2 = r2 / ell2;
    return (sigma2 / (ell2 * ell2)) * (3.0 - 6.0 * rho2 + rho2 * rho2) * exp(-0.5 * r2 / ell2);
}

/* K_fu(x, x') = Cov[f(x), f''(x')] = (σ²/ℓ²)(ρ² - 1) exp(-ρ²/2)  [symmetric] */
double cross_covariance_f_u(double x, double xp, double sigma2, double ell) {
    double r = x - xp;
    double r2 = r * r;
    double ell2 = ell * ell;
    return (sigma2 / ell2) * (r2 / ell2 - 1.0) * exp(-0.5 * r2 / ell2);
}

/* K_hu(x, x') = Cov[f'(x), f''(x')] = (σ²/ℓ⁴) r (3 - r²/ℓ²) exp(-r²/(2ℓ²))  [anti-sym in args] */
double cross_covariance_h_u(double x, double xp, double sigma2, double ell) {
    double r = x - xp;
    double r2 = r * r;
    double ell2 = ell * ell;
    return (sigma2 / (ell2 * ell2)) * r * (3.0 - r2 / ell2) * exp(-0.5 * r2 / ell2);
}

/* K_gu(y, x') = Cov[g(y), f''(x')] = (σ²/ℓ²)[-x' exp(-x'²/(2ℓ²)) - (y-x') exp(-(y-x')²/(2ℓ²))] */
double cross_covariance_g_u(double y, double xp, double sigma2, double ell) {
    double ell2 = ell * ell;
    double exp_xp = exp(-xp * xp / (2.0 * ell2));
    double diff = y - xp;
    double exp_diff = exp(-diff * diff / (2.0 * ell2));
    return (sigma2 / ell2) * (-xp * exp_xp - diff * exp_diff);
}

// =============================================================================
// Second-derivative (u = f'') kernel functions — Matérn 5/2
// =============================================================================

/* K_uu for Matérn 5/2: (25σ²/3ℓ⁶)(3ℓ² - 5√5 ℓ r + 5r²) exp(-√5 r/ℓ) */
static double matern52_second_derivative_kernel(double x, double xp, double sigma2, double ell) {
    double r = fabs(x - xp);
    double ell2 = ell * ell;
    return (25.0 / 3.0) * sigma2 * (3.0 * ell2 - 5.0 * sqrt(5.0) * ell * r + 5.0 * r * r)
           * exp(-sqrt(5.0) * r / ell) / (ell2 * ell2 * ell2);
}

/* K_fu for Matérn 5/2: (5σ²/3ℓ⁴)(-ℓ² - √5 ℓ r + 5r²) exp(-√5 r/ℓ) */
static double matern52_cross_covariance_f_u(double x, double xp, double sigma2, double ell) {
    double r = fabs(x - xp);
    double ell2 = ell * ell;
    return (5.0 / 3.0) * sigma2 * (-ell2 - sqrt(5.0) * ell * r + 5.0 * r * r)
           * exp(-sqrt(5.0) * r / ell) / (ell2 * ell2);
}

/* K_hu for Matérn 5/2: sign(x-xp) · (25σ²/3ℓ⁵) r (3ℓ - √5 r) exp(-√5 r/ℓ) */
static double matern52_cross_covariance_h_u(double x, double xp, double sigma2, double ell) {
    double diff = x - xp;
    double r = fabs(diff);
    double sgn = (diff > 0.0) - (diff < 0.0);
    return sgn * (25.0 / 3.0) * r * sigma2 * (3.0 * ell - sqrt(5.0) * r)
           * exp(-sqrt(5.0) * r / ell) / (ell * ell * ell * ell * ell);
}

/* K_gu for Matérn 5/2: ∫₀ʸ K_fu(x, xp) dx — piecewise in three regions */
static double matern52_cross_covariance_g_u(double y, double xp, double sigma2, double ell) {
    double ell3 = ell * ell * ell;
    double s5 = sqrt(5.0);
    if (xp <= 0.0) {
        /* region: x ≤ 0, |x - xp| = xp - x for all x in [0,y] */
        return -(5.0 / 3.0) * sigma2 / ell3 * (
            xp * (ell - s5 * xp) * exp(s5 * xp / ell)
            - (ell - s5 * (xp - y)) * (xp - y) * exp(s5 * (xp - y) / ell)
        );
    } else if (y <= xp) {
        /* region: y ≤ xp, |x - xp| = xp - x for all x in [0,y] */
        return -(5.0 / 3.0) * sigma2 / ell3 * (
            xp * (ell + s5 * xp) * exp(s5 * (xp - y) / ell)
            - (ell + s5 * (xp - y)) * (xp - y) * exp(s5 * xp / ell)
        ) * exp(-s5 * (2.0 * xp - y) / ell);
    } else {
        /* region: 0 ≤ xp ≤ y — integral splits at x = xp */
        return -(5.0 / 3.0) * sigma2 / ell3 * (
            xp * (ell + s5 * xp)
            - (ell - s5 * (xp - y)) * (xp - y) * exp(s5 * (2.0 * xp - y) / ell)
        ) * exp(-s5 * xp / ell);
    }
}

// =============================================================================
// Second-derivative (u = f'') kernel functions — Periodic
// No K_gu: periodic kernel does not support integral observations.
// All formulas derived from k(t) = σ² exp(-2 sin²(πt/p) / ℓ²), t = x - x'.
// =============================================================================

/* K_fu for periodic: ∂²k/∂x'² = k''(t)
 * = (4π²σ²/ℓ⁴p²)(-ℓ² cos(2πt/p) - ½cos(4πt/p) + ½) exp(-2sin²(πt/p)/ℓ²) */
static double periodic_cross_covariance_f_u(double x, double xp, double sigma2, double ell, double period) {
    double t = x - xp;
    double ell2 = ell * ell;
    double ell4 = ell2 * ell2;
    double p2 = period * period;
    double sin_t = sin(M_PI * t / period);
    double exp_term = exp(-2.0 * sin_t * sin_t / ell2);
    return 4.0 * M_PI * M_PI * sigma2 / (ell4 * p2)
           * (-ell2 * cos(2.0 * M_PI * t / period)
              - 0.5 * cos(4.0 * M_PI * t / period) + 0.5)
           * exp_term;
}

/* K_uu for periodic: ∂⁴k/∂t⁴
 * = (16π⁴σ²/ℓ⁸p⁴)(ℓ⁶cos(2πt/p) + ℓ⁴(28sin⁴(πt/p)-28sin²(πt/p)+3)
 *   - (3/2)ℓ²(cos(2πt/p)-cos(6πt/p)) + 16sin⁴(πt/p)cos⁴(πt/p)) exp(-2sin²(πt/p)/ℓ²) */
static double periodic_second_derivative_kernel(double x, double xp, double sigma2, double ell, double period) {
    double t = x - xp;
    double ell2 = ell * ell;
    double ell4 = ell2 * ell2;
    double ell6 = ell4 * ell2;
    double ell8 = ell4 * ell4;
    double p4 = period * period * period * period;
    double sin_t  = sin(M_PI * t / period);
    double cos_t  = cos(M_PI * t / period);
    double sin2   = sin_t * sin_t;
    double sin4   = sin2 * sin2;
    double cos4   = cos_t * cos_t * cos_t * cos_t;
    double exp_term = exp(-2.0 * sin2 / ell2);
    double bracket = ell6 * cos(2.0 * M_PI * t / period)
                   + ell4 * (28.0 * sin4 - 28.0 * sin2 + 3.0)
                   - 1.5 * ell2 * (cos(2.0 * M_PI * t / period) - cos(6.0 * M_PI * t / period))
                   + 16.0 * sin4 * cos4;
    return 16.0 * M_PI * M_PI * M_PI * M_PI * sigma2 / (ell8 * p4) * bracket * exp_term;
}

/* K_hu for periodic: ∂³k/∂t³  (antisymmetric in t)
 * = (16π³σ²/ℓ⁶p³)(ℓ⁴ + 3ℓ²cos(2πt/p) + ½cos(4πt/p) - ½) sin(πt/p)cos(πt/p) exp(-2sin²(πt/p)/ℓ²) */
static double periodic_cross_covariance_h_u(double x, double xp, double sigma2, double ell, double period) {
    double t = x - xp;
    double ell2 = ell * ell;
    double ell4 = ell2 * ell2;
    double ell6 = ell4 * ell2;
    double p3 = period * period * period;
    double sin_t = sin(M_PI * t / period);
    double cos_t = cos(M_PI * t / period);
    double sin2  = sin_t * sin_t;
    double exp_term = exp(-2.0 * sin2 / ell2);
    double bracket = ell4 + 3.0 * ell2 * cos(2.0 * M_PI * t / period)
                   + 0.5 * cos(4.0 * M_PI * t / period) - 0.5;
    return 16.0 * M_PI * M_PI * M_PI * sigma2 / (ell6 * p3) * bracket * sin_t * cos_t * exp_term;
}

/* Dispatcher: K_uu — RBF, Matérn 5/2, Periodic, Locally-Periodic */
static inline double compute_k_uu(double x, double xp, double sigma2, double ell, double ell_p, double period, KernelType kernel) {
    if (kernel == KERNEL_RBF)              return rbf_second_derivative_kernel(x, xp, sigma2, ell);
    if (kernel == KERNEL_MATERN52)         return matern52_second_derivative_kernel(x, xp, sigma2, ell);
    if (kernel == KERNEL_PERIODIC)         return periodic_second_derivative_kernel(x, xp, sigma2, ell_p, period);
    if (kernel == KERNEL_LOCALLY_PERIODIC) return locally_periodic_second_derivative_kernel(x, xp, sigma2, ell, ell_p, period);
    fprintf(stderr, "ERROR: u=f'' not supported for this kernel.\n");
    return 0.0;
}

/* Dispatcher: K_fu — RBF, Matérn 5/2, Periodic, Locally-Periodic */
static inline double compute_k_fu(double x, double xp, double sigma2, double ell, double ell_p, double period, KernelType kernel) {
    if (kernel == KERNEL_RBF)              return cross_covariance_f_u(x, xp, sigma2, ell);
    if (kernel == KERNEL_MATERN52)         return matern52_cross_covariance_f_u(x, xp, sigma2, ell);
    if (kernel == KERNEL_PERIODIC)         return periodic_cross_covariance_f_u(x, xp, sigma2, ell_p, period);
    if (kernel == KERNEL_LOCALLY_PERIODIC) return locally_periodic_cross_covariance_f_u(x, xp, sigma2, ell, ell_p, period);
    fprintf(stderr, "ERROR: u=f'' not supported for this kernel.\n");
    return 0.0;
}

/* Dispatcher: K_hu — RBF, Matérn 5/2, Periodic, Locally-Periodic */
static inline double compute_k_hu(double x, double xp, double sigma2, double ell, double ell_p, double period, KernelType kernel) {
    if (kernel == KERNEL_RBF)              return cross_covariance_h_u(x, xp, sigma2, ell);
    if (kernel == KERNEL_MATERN52)         return matern52_cross_covariance_h_u(x, xp, sigma2, ell);
    if (kernel == KERNEL_PERIODIC)         return periodic_cross_covariance_h_u(x, xp, sigma2, ell_p, period);
    if (kernel == KERNEL_LOCALLY_PERIODIC) return locally_periodic_cross_covariance_h_u(x, xp, sigma2, ell, ell_p, period);
    fprintf(stderr, "ERROR: u=f'' not supported for this kernel.\n");
    return 0.0;
}

/* Dispatcher: K_gu — RBF and Matérn 5/2 only (periodic/locally-periodic don't support g) */
static inline double compute_k_gu(double y, double xp, double sigma2, double ell, double ell_p, double period, KernelType kernel) {
    (void)ell_p; (void)period;
    if (kernel == KERNEL_RBF)      return cross_covariance_g_u(y, xp, sigma2, ell);
    if (kernel == KERNEL_MATERN52) return matern52_cross_covariance_g_u(y, xp, sigma2, ell);
    fprintf(stderr, "ERROR: K_gu not supported for this kernel.\n");
    return 0.0;
}

// =============================================================================
// Mean Function Helpers
// =============================================================================

/**
 * Evaluate mean function for f at a single point
 * For constant mean c: m_f(x) = c
 */
static double eval_mean_f(double x, const GPMeanSpec *mean) {
    if (mean == NULL || mean->type == MEAN_ZERO) {
        return 0.0;
    }
    
    switch (mean->type) {
        case MEAN_CONSTANT:
            return mean->params.constant.value;
        default:
            return 0.0;
    }
}

/**
 * Evaluate mean function for g=∫f at a single point
 * For constant c: m_g(x) = ∫₀ˣ c dt = c*x
 */
static double eval_mean_g(double x, const GPMeanSpec *mean) {
    if (mean == NULL || mean->type == MEAN_ZERO) {
        return 0.0;
    }
    
    switch (mean->type) {
        case MEAN_CONSTANT:
            return mean->params.constant.value * x;
        default:
            return 0.0;
    }
}

/**
 * Evaluate mean function for h=f' at a single point
 * For constant c: m_h(x) = dc/dx = 0
 */
static double eval_mean_h(double x, const GPMeanSpec *mean) {
    if (mean == NULL || mean->type == MEAN_ZERO) {
        return 0.0;
    }

    switch (mean->type) {
        case MEAN_CONSTANT:
            return 0.0;  // Derivative of constant is zero
        default:
            return 0.0;
    }
}

static double eval_mean_u(double x, const GPMeanSpec *mean) {
    (void)x;
    (void)mean;
    return 0.0;  // Second derivative of any mean is zero
}

// =============================================================================
// Sampling Functions
// =============================================================================

int draw_joint_gp_samples(
    const double *x_f, int n_f,
    const double *x_g, int n_g,
    double sigma2, double ell, double ell_p, double period,
    KernelType kernel,
    int n_samples, unsigned long seed,
    double *f_samples,
    double *g_samples,
    const GPMeanSpec *mean
) {
    // Validate: periodic kernels do NOT support integral observations
    if ((kernel == KERNEL_PERIODIC || kernel == KERNEL_LOCALLY_PERIODIC) && n_g > 0) {
        fprintf(stderr, "ERROR: Periodic kernels do not support integral observations (x_g).\n");
        fprintf(stderr, "       Integral observations require numerical quadrature (not yet implemented).\n");
        fprintf(stderr, "       Please use only f (function) and h (derivative) observations.\n");
        return -1;
    }

    int n_total = n_f + n_g;

    // Initialize random number generator
    gsl_rng *rng = gsl_rng_alloc(gsl_rng_mt19937);
    gsl_rng_set(rng, seed);

    // Allocate joint covariance matrix
    gsl_matrix *K = gsl_matrix_alloc(n_total, n_total);

    // Build K_ff block (top-left: n_f × n_f)
    for (int i = 0; i < n_f; i++) {
        for (int j = 0; j < n_f; j++) {
            double k_val = compute_k_ff(x_f[i], x_f[j], sigma2, ell, ell_p, period, kernel);
            gsl_matrix_set(K, i, j, k_val);
        }
    }

    // Build K_gg block (bottom-right: n_g × n_g)
    for (int i = 0; i < n_g; i++) {
        for (int j = 0; j < n_g; j++) {
            double k_val = compute_k_gg(x_g[i], x_g[j], sigma2, ell, ell_p, period, kernel);
            gsl_matrix_set(K, n_f + i, n_f + j, k_val);
        }
    }

    // Build K_fg and K_gf blocks (cross-covariance, symmetric)
    for (int i = 0; i < n_f; i++) {
        for (int j = 0; j < n_g; j++) {
            double k_val = compute_k_fg(x_f[i], x_g[j], sigma2, ell, ell_p, period, kernel);
            gsl_matrix_set(K, i, n_f + j, k_val);        // K_fg
            gsl_matrix_set(K, n_f + j, i, k_val);        // K_gf (transpose)
        }
    }
    
    // Add scale-aware jitter for numerical stability
    double regularization = 1e-6;
    double min_jitter = 1e-14;

    // Find max diagonal for each block
    double max_diag_f = 0.0, max_diag_g = 0.0;
    for (int i = 0; i < n_f; i++) {
        double val = gsl_matrix_get(K, i, i);
        if (val > max_diag_f) max_diag_f = val;
    }
    for (int i = 0; i < n_g; i++) {
        double val = gsl_matrix_get(K, n_f + i, n_f + i);
        if (val > max_diag_g) max_diag_g = val;
    }

    // Apply scale-proportional jitter
    double jitter_f = fmax(regularization * max_diag_f, min_jitter);
    double jitter_g = fmax(regularization * max_diag_g, min_jitter);
    for (int i = 0; i < n_f; i++) {
        double val = gsl_matrix_get(K, i, i);
        gsl_matrix_set(K, i, i, val + jitter_f);
    }
    for (int i = 0; i < n_g; i++) {
        double val = gsl_matrix_get(K, n_f + i, n_f + i);
        gsl_matrix_set(K, n_f + i, n_f + i, val + jitter_g);
    }

    // Save a copy for fallback retry (handles singularity at x_g=0)
    gsl_matrix *K_copy = gsl_matrix_alloc(n_total, n_total);
    gsl_matrix_memcpy(K_copy, K);

    // Cholesky decomposition with fallback
    int chol_status = cholesky_with_fallback(K, K_copy, n_total);
    gsl_matrix_free(K_copy);
    if (chol_status != 0) {
        gsl_matrix_free(K);
        gsl_rng_free(rng);
        return -1;
    }

    // Allocate vectors for sampling
    gsl_vector *z = gsl_vector_alloc(n_total);      // Standard normal samples
    gsl_vector *sample = gsl_vector_alloc(n_total); // GP sample = L * z
    
    // Draw samples
    for (int s = 0; s < n_samples; s++) {
        // Draw standard normal samples z ~ N(0, I)
        for (int i = 0; i < n_total; i++) {
            double z_i = gsl_ran_gaussian(rng, 1.0);
            gsl_vector_set(z, i, z_i);
        }
        
        // Compute sample = L * z (using lower triangular matrix)
        gsl_vector_memcpy(sample, z);
        gsl_blas_dtrmv(CblasLower, CblasNoTrans, CblasNonUnit, K, sample);
        
        // Extract f samples (first n_f elements) and add mean
        for (int i = 0; i < n_f; i++) {
            double centered_sample = gsl_vector_get(sample, i);
            double mean_value = eval_mean_f(x_f[i], mean);
            f_samples[s * n_f + i] = centered_sample + mean_value;
        }
        
        // Extract g samples (next n_g elements) and add mean
        for (int i = 0; i < n_g; i++) {
            double centered_sample = gsl_vector_get(sample, n_f + i);
            double mean_value = eval_mean_g(x_g[i], mean);
            g_samples[s * n_g + i] = centered_sample + mean_value;
        }
    }
    
    // Cleanup
    gsl_vector_free(z);
    gsl_vector_free(sample);
    gsl_matrix_free(K);
    gsl_rng_free(rng);

    return 0;
}

int draw_gp_samples_flexible(
    const GPSamplingSpec *spec,
    double sigma2, double ell, double ell_p, double period,
    KernelType kernel,
    int n_samples, unsigned long seed,
    double *f_samples, double *g_samples, double *h_samples, double *u_samples,
    const GPMeanSpec *mean
) {
    // Extract dimensions based on flags
    int n_f = (spec->flags & GP_SAMPLE_F) ? spec->n_f : 0;
    int n_g = (spec->flags & GP_SAMPLE_G) ? spec->n_g : 0;
    int n_h = (spec->flags & GP_SAMPLE_H) ? spec->n_h : 0;
    int n_u = (spec->flags & GP_SAMPLE_U) ? spec->n_u : 0;
    int n_total = n_f + n_g + n_h + n_u;

    if (n_total == 0) {
        fprintf(stderr, "No samples requested (all flags zero or counts zero)\n");
        return -1;
    }

    // Validate: periodic kernels do NOT support integral observations
    if ((kernel == KERNEL_PERIODIC || kernel == KERNEL_LOCALLY_PERIODIC) && n_g > 0) {
        fprintf(stderr, "ERROR: Periodic kernels do not support integral observations (x_g).\n");
        fprintf(stderr, "       Integral observations require numerical quadrature (not yet implemented).\n");
        fprintf(stderr, "       Please use only f (function) and h (derivative) observations.\n");
        return -1;
    }

    // Initialize random number generator
    gsl_rng *rng = gsl_rng_alloc(gsl_rng_mt19937);
    gsl_rng_set(rng, seed);

    // Allocate joint covariance matrix
    gsl_matrix *K = gsl_matrix_alloc(n_total, n_total);

    // Index offsets for each block
    int offset_f = 0;
    int offset_g = n_f;
    int offset_h = n_f + n_g;
    int offset_u = n_f + n_g + n_h;

    // Build K_ff block (n_f × n_f)
    if (n_f > 0) {
        for (int i = 0; i < n_f; i++) {
            for (int j = 0; j < n_f; j++) {
                double k_val = compute_k_ff(spec->x_f[i], spec->x_f[j], sigma2, ell, ell_p, period, kernel);
                gsl_matrix_set(K, offset_f + i, offset_f + j, k_val);
            }
        }
    }

    // Build K_gg block (n_g × n_g)
    if (n_g > 0) {
        for (int i = 0; i < n_g; i++) {
            for (int j = 0; j < n_g; j++) {
                double k_val = compute_k_gg(spec->x_g[i], spec->x_g[j], sigma2, ell, ell_p, period, kernel);
                gsl_matrix_set(K, offset_g + i, offset_g + j, k_val);
            }
        }
    }

    // Build K_hh block (n_h × n_h)
    if (n_h > 0) {
        for (int i = 0; i < n_h; i++) {
            for (int j = 0; j < n_h; j++) {
                double k_val = compute_k_hh(spec->x_h[i], spec->x_h[j], sigma2, ell, ell_p, period, kernel);
                gsl_matrix_set(K, offset_h + i, offset_h + j, k_val);
            }
        }
    }

    // Build K_uu block (n_u × n_u)
    if (n_u > 0) {
        for (int i = 0; i < n_u; i++) {
            for (int j = 0; j < n_u; j++) {
                double k_val = compute_k_uu(spec->x_u[i], spec->x_u[j], sigma2, ell, ell_p, period, kernel);
                gsl_matrix_set(K, offset_u + i, offset_u + j, k_val);
            }
        }
    }

    // Build K_fg and K_gf blocks (symmetric cross-covariance)
    if (n_f > 0 && n_g > 0) {
        for (int i = 0; i < n_f; i++) {
            for (int j = 0; j < n_g; j++) {
                double k_val = compute_k_fg(spec->x_f[i], spec->x_g[j], sigma2, ell, ell_p, period, kernel);
                gsl_matrix_set(K, offset_f + i, offset_g + j, k_val);  // K_fg
                gsl_matrix_set(K, offset_g + j, offset_f + i, k_val);  // K_gf (transpose)
            }
        }
    }

    // Build K_fh and K_hf blocks
    if (n_f > 0 && n_h > 0) {
        for (int i = 0; i < n_f; i++) {
            for (int j = 0; j < n_h; j++) {
                double k_val = compute_k_fh(spec->x_f[i], spec->x_h[j], sigma2, ell, ell_p, period, kernel);
                gsl_matrix_set(K, offset_f + i, offset_h + j, k_val);  // K_fh
                gsl_matrix_set(K, offset_h + j, offset_f + i, k_val);  // K_hf = K_fh^T (covariance symmetry)
            }
        }
    }

    // Build K_gh and K_hg blocks
    if (n_g > 0 && n_h > 0) {
        for (int i = 0; i < n_g; i++) {
            for (int j = 0; j < n_h; j++) {
                double k_val = compute_k_gh(spec->x_g[i], spec->x_h[j], sigma2, ell, ell_p, period, kernel);
                gsl_matrix_set(K, offset_g + i, offset_h + j, k_val);  // K_gh
                gsl_matrix_set(K, offset_h + j, offset_g + i, k_val);  // K_hg (transpose, symmetric)
            }
        }
    }

    // Build K_fu and K_uf blocks (symmetric)
    if (n_f > 0 && n_u > 0) {
        for (int i = 0; i < n_f; i++) {
            for (int j = 0; j < n_u; j++) {
                double k_val = compute_k_fu(spec->x_f[i], spec->x_u[j], sigma2, ell, ell_p, period, kernel);
                gsl_matrix_set(K, offset_f + i, offset_u + j, k_val);  // K_fu
                gsl_matrix_set(K, offset_u + j, offset_f + i, k_val);  // K_uf (= K_fu, symmetric)
            }
        }
    }

    // Build K_gu and K_ug blocks
    if (n_g > 0 && n_u > 0) {
        for (int i = 0; i < n_g; i++) {
            for (int j = 0; j < n_u; j++) {
                double k_val = compute_k_gu(spec->x_g[i], spec->x_u[j], sigma2, ell, ell_p, period, kernel);
                gsl_matrix_set(K, offset_g + i, offset_u + j, k_val);  // K_gu
                gsl_matrix_set(K, offset_u + j, offset_g + i, k_val);  // K_ug (symmetric)
            }
        }
    }

    // Build K_hu and K_uh blocks (anti-symmetric in arguments, but Cov[h,u]=Cov[u,h] by symmetry)
    if (n_h > 0 && n_u > 0) {
        for (int i = 0; i < n_h; i++) {
            for (int j = 0; j < n_u; j++) {
                double k_val = compute_k_hu(spec->x_h[i], spec->x_u[j], sigma2, ell, ell_p, period, kernel);
                gsl_matrix_set(K, offset_h + i, offset_u + j, k_val);  // K_hu
                gsl_matrix_set(K, offset_u + j, offset_h + i, k_val);  // K_uh = K_hu^T
            }
        }
    }

    // Add scale-aware jitter for numerical stability
    //
    // When ell is large, covariance matrices become ill-conditioned (nearly rank-1).
    // Using jitter proportional to max diagonal bounds the condition number.
    //
    double regularization = 1e-6;  // Target condition number ~ 1e6
    double min_jitter = 1e-14;     // Absolute minimum

    // Find max diagonal for each block
    double max_diag_f = 0.0, max_diag_g = 0.0, max_diag_h = 0.0, max_diag_u = 0.0;
    for (int i = 0; i < n_f; i++) {
        double val = gsl_matrix_get(K, offset_f + i, offset_f + i);
        if (val > max_diag_f) max_diag_f = val;
    }
    for (int i = 0; i < n_g; i++) {
        double val = gsl_matrix_get(K, offset_g + i, offset_g + i);
        if (val > max_diag_g) max_diag_g = val;
    }
    for (int i = 0; i < n_h; i++) {
        double val = gsl_matrix_get(K, offset_h + i, offset_h + i);
        if (val > max_diag_h) max_diag_h = val;
    }
    for (int i = 0; i < n_u; i++) {
        double val = gsl_matrix_get(K, offset_u + i, offset_u + i);
        if (val > max_diag_u) max_diag_u = val;
    }

    // Compute block-specific jitter
    double jitter_f = fmax(regularization * max_diag_f, min_jitter);
    double jitter_g = fmax(regularization * max_diag_g, min_jitter);
    double jitter_h = fmax(regularization * max_diag_h, min_jitter);
    double jitter_u = fmax(regularization * max_diag_u, min_jitter);

    // Apply jitter to each block
    for (int i = 0; i < n_f; i++) {
        double val = gsl_matrix_get(K, offset_f + i, offset_f + i);
        gsl_matrix_set(K, offset_f + i, offset_f + i, val + jitter_f);
    }
    for (int i = 0; i < n_g; i++) {
        double val = gsl_matrix_get(K, offset_g + i, offset_g + i);
        gsl_matrix_set(K, offset_g + i, offset_g + i, val + jitter_g);
    }
    for (int i = 0; i < n_h; i++) {
        double val = gsl_matrix_get(K, offset_h + i, offset_h + i);
        gsl_matrix_set(K, offset_h + i, offset_h + i, val + jitter_h);
    }
    for (int i = 0; i < n_u; i++) {
        double val = gsl_matrix_get(K, offset_u + i, offset_u + i);
        gsl_matrix_set(K, offset_u + i, offset_u + i, val + jitter_u);
    }

    // Save a copy for fallback retry (handles singularity at x_g=0)
    gsl_matrix *K_copy = gsl_matrix_alloc(n_total, n_total);
    gsl_matrix_memcpy(K_copy, K);

    // Cholesky decomposition with fallback
    int chol_status = cholesky_with_fallback(K, K_copy, n_total);
    gsl_matrix_free(K_copy);
    if (chol_status != 0) {
        gsl_matrix_free(K);
        gsl_rng_free(rng);
        return -1;
    }

    // Allocate vectors for sampling
    gsl_vector *z = gsl_vector_alloc(n_total);
    gsl_vector *sample = gsl_vector_alloc(n_total);

    // Draw samples
    for (int s = 0; s < n_samples; s++) {
        // Draw standard normal samples z ~ N(0, I)
        for (int i = 0; i < n_total; i++) {
            double z_i = gsl_ran_gaussian(rng, 1.0);
            gsl_vector_set(z, i, z_i);
        }

        // Compute sample = L * z (using lower triangular matrix)
        gsl_vector_memcpy(sample, z);
        gsl_blas_dtrmv(CblasLower, CblasNoTrans, CblasNonUnit, K, sample);

        // Extract f samples and add mean
        if (n_f > 0 && f_samples != NULL) {
            for (int i = 0; i < n_f; i++) {
                double centered_sample = gsl_vector_get(sample, offset_f + i);
                double mean_value = eval_mean_f(spec->x_f[i], mean);
                f_samples[s * n_f + i] = centered_sample + mean_value;
            }
        }

        // Extract g samples and add mean
        if (n_g > 0 && g_samples != NULL) {
            for (int i = 0; i < n_g; i++) {
                double centered_sample = gsl_vector_get(sample, offset_g + i);
                double mean_value = eval_mean_g(spec->x_g[i], mean);
                g_samples[s * n_g + i] = centered_sample + mean_value;
            }
        }

        // Extract h samples and add mean
        if (n_h > 0 && h_samples != NULL) {
            for (int i = 0; i < n_h; i++) {
                double centered_sample = gsl_vector_get(sample, offset_h + i);
                double mean_value = eval_mean_h(spec->x_h[i], mean);
                h_samples[s * n_h + i] = centered_sample + mean_value;
            }
        }

        // Extract u samples (mean of u is always 0)
        if (n_u > 0 && u_samples != NULL) {
            for (int i = 0; i < n_u; i++) {
                u_samples[s * n_u + i] = gsl_vector_get(sample, offset_u + i);
            }
        }
    }

    // Cleanup
    gsl_vector_free(z);
    gsl_vector_free(sample);
    gsl_matrix_free(K);
    gsl_rng_free(rng);

    return 0;
}

// Helper function to fill a covariance block between two sets of points
// Uses GPFunctionType enum for type safety instead of magic integers
static void fill_covariance_block(
    gsl_matrix *K,
    int row_offset, int col_offset,
    const double *x1, int n1, GPFunctionType type1,
    const double *x2, int n2, GPFunctionType type2,
    double sigma2, double ell, double ell_p, double period,
    KernelType kernel
) {
    for (int i = 0; i < n1; i++) {
        for (int j = 0; j < n2; j++) {
            double k_val = 0.0;

            if (type1 == GP_TYPE_F && type2 == GP_TYPE_F) {
                // K_ff
                k_val = compute_k_ff(x1[i], x2[j], sigma2, ell, ell_p, period, kernel);
            } else if (type1 == GP_TYPE_G && type2 == GP_TYPE_G) {
                // K_gg
                k_val = compute_k_gg(x1[i], x2[j], sigma2, ell, ell_p, period, kernel);
            } else if (type1 == GP_TYPE_H && type2 == GP_TYPE_H) {
                // K_hh
                k_val = compute_k_hh(x1[i], x2[j], sigma2, ell, ell_p, period, kernel);
            } else if (type1 == GP_TYPE_F && type2 == GP_TYPE_G) {
                // K_fg
                k_val = compute_k_fg(x1[i], x2[j], sigma2, ell, ell_p, period, kernel);
            } else if (type1 == GP_TYPE_G && type2 == GP_TYPE_F) {
                // K_gf = K_fg^T
                k_val = compute_k_fg(x2[j], x1[i], sigma2, ell, ell_p, period, kernel);
            } else if (type1 == GP_TYPE_F && type2 == GP_TYPE_H) {
                // K_fh
                k_val = compute_k_fh(x1[i], x2[j], sigma2, ell, ell_p, period, kernel);
            } else if (type1 == GP_TYPE_H && type2 == GP_TYPE_F) {
                // K_hf = K_fh^T (transpose by covariance symmetry: Cov[A,B] = Cov[B,A])
                k_val = compute_k_fh(x2[j], x1[i], sigma2, ell, ell_p, period, kernel);
            } else if (type1 == GP_TYPE_G && type2 == GP_TYPE_H) {
                // K_gh
                k_val = compute_k_gh(x1[i], x2[j], sigma2, ell, ell_p, period, kernel);
            } else if (type1 == GP_TYPE_H && type2 == GP_TYPE_G) {
                // K_hg = K_gh^T
                k_val = compute_k_gh(x2[j], x1[i], sigma2, ell, ell_p, period, kernel);
            } else if (type1 == GP_TYPE_U && type2 == GP_TYPE_U) {
                k_val = compute_k_uu(x1[i], x2[j], sigma2, ell, ell_p, period, kernel);
            } else if (type1 == GP_TYPE_F && type2 == GP_TYPE_U) {
                k_val = compute_k_fu(x1[i], x2[j], sigma2, ell, ell_p, period, kernel);
            } else if (type1 == GP_TYPE_U && type2 == GP_TYPE_F) {
                k_val = compute_k_fu(x2[j], x1[i], sigma2, ell, ell_p, period, kernel);  // K_fu is symmetric
            } else if (type1 == GP_TYPE_H && type2 == GP_TYPE_U) {
                k_val = compute_k_hu(x1[i], x2[j], sigma2, ell, ell_p, period, kernel);
            } else if (type1 == GP_TYPE_U && type2 == GP_TYPE_H) {
                k_val = compute_k_hu(x2[j], x1[i], sigma2, ell, ell_p, period, kernel);  // K_uh = K_hu^T
            } else if (type1 == GP_TYPE_G && type2 == GP_TYPE_U) {
                k_val = compute_k_gu(x1[i], x2[j], sigma2, ell, ell_p, period, kernel);
            } else if (type1 == GP_TYPE_U && type2 == GP_TYPE_G) {
                k_val = compute_k_gu(x2[j], x1[i], sigma2, ell, ell_p, period, kernel);  // K_ug = K_gu^T
            }

            gsl_matrix_set(K, row_offset + i, col_offset + j, k_val);
        }
    }
}

/**
 * Add noise covariance to a block of the observation covariance matrix.
 *
 * Supports three noise types:
 *   - NOISE_SCALAR: Single variance σ² added to diagonal (noise_cov points to 1 double)
 *   - NOISE_DIAGONAL: Per-observation variances (noise_cov points to n doubles)
 *   - NOISE_FULL: Full covariance matrix (noise_cov points to n×n doubles, row-major)
 *
 * @param K          The covariance matrix to modify (in place)
 * @param offset     Row/column offset for the block
 * @param noise_cov  Pointer to noise data
 * @param noise_type How to interpret noise_cov
 * @param n          Number of observations in this block
 */
static void add_noise_covariance(
    gsl_matrix *K,
    int offset,
    const double *noise_cov,
    NoiseType noise_type,
    int n
) {
    if (n <= 0 || noise_cov == NULL) {
        return;
    }

    switch (noise_type) {
        case NOISE_SCALAR:
            // Add σ² to diagonal elements
            for (int i = 0; i < n; i++) {
                double val = gsl_matrix_get(K, offset + i, offset + i);
                gsl_matrix_set(K, offset + i, offset + i, val + noise_cov[0]);
            }
            break;

        case NOISE_DIAGONAL:
            // Add σᵢ² to each diagonal element
            for (int i = 0; i < n; i++) {
                double val = gsl_matrix_get(K, offset + i, offset + i);
                gsl_matrix_set(K, offset + i, offset + i, val + noise_cov[i]);
            }
            break;

        case NOISE_FULL:
            // Add full covariance matrix (row-major storage)
            for (int i = 0; i < n; i++) {
                for (int j = 0; j < n; j++) {
                    double k_val = gsl_matrix_get(K, offset + i, offset + j);
                    double noise_val = noise_cov[i * n + j];
                    gsl_matrix_set(K, offset + i, offset + j, k_val + noise_val);
                }
            }
            break;
    }
}

int draw_gp_posterior_samples(
    const GPObservations *obs,
    const GPSamplingSpec *pred,
    double sigma2, double ell, double ell_p, double period,
    KernelType kernel,
    int n_samples, unsigned long seed,
    double *f_samples, double *g_samples, double *h_samples, double *u_samples,
    double *f_mean, double *g_mean, double *h_mean, double *u_mean,
    double *f_std, double *g_std, double *h_std, double *u_std,
    const GPMeanSpec *mean
) {
    // Get observation counts
    int n_obs_f = (obs->x_f != NULL && obs->y_f != NULL) ? obs->n_f : 0;
    int n_obs_g = (obs->x_g != NULL && obs->y_g != NULL) ? obs->n_g : 0;
    int n_obs_h = (obs->x_h != NULL && obs->y_h != NULL) ? obs->n_h : 0;
    int n_obs_u = (obs->x_u != NULL && obs->y_u != NULL) ? obs->n_u : 0;
    int n_obs = n_obs_f + n_obs_g + n_obs_h + n_obs_u;

    // Get prediction counts
    int n_pred_f = (pred->flags & GP_SAMPLE_F) ? pred->n_f : 0;
    int n_pred_g = (pred->flags & GP_SAMPLE_G) ? pred->n_g : 0;
    int n_pred_h = (pred->flags & GP_SAMPLE_H) ? pred->n_h : 0;
    int n_pred_u = (pred->flags & GP_SAMPLE_U) ? pred->n_u : 0;
    int n_pred = n_pred_f + n_pred_g + n_pred_h + n_pred_u;

    if (n_obs == 0) {
        fprintf(stderr, "No observations provided\n");
        return -1;
    }
    if (n_pred == 0) {
        fprintf(stderr, "No prediction points provided\n");
        return -2;
    }

    // Validate: periodic kernels do NOT support integral observations or predictions
    if (kernel == KERNEL_PERIODIC || kernel == KERNEL_LOCALLY_PERIODIC) {
        if (n_obs_g > 0) {
            fprintf(stderr, "ERROR: Periodic kernels do not support integral observations (x_g).\n");
            fprintf(stderr, "       Integral observations require numerical quadrature (not yet implemented).\n");
            fprintf(stderr, "       Please use only f (function) and h (derivative) observations.\n");
            return -1;
        }
        if (n_pred_g > 0) {
            fprintf(stderr, "ERROR: Periodic kernels do not support integral predictions (x_g).\n");
            fprintf(stderr, "       Integral predictions require numerical quadrature (not yet implemented).\n");
            fprintf(stderr, "       Please use only f (function) and h (derivative) predictions.\n");
            return -2;
        }
    }

    // Initialize random number generator
    gsl_rng *rng = gsl_rng_alloc(gsl_rng_mt19937);
    gsl_rng_set(rng, seed);

    // Offsets for observation blocks
    int obs_offset_f = 0;
    int obs_offset_g = n_obs_f;
    int obs_offset_h = n_obs_f + n_obs_g;
    int obs_offset_u = n_obs_f + n_obs_g + n_obs_h;

    // Offsets for prediction blocks
    int pred_offset_f = 0;
    int pred_offset_g = n_pred_f;
    int pred_offset_h = n_pred_f + n_pred_g;
    int pred_offset_u = n_pred_f + n_pred_g + n_pred_h;

    // =========================================================================
    // 1. Build K_obs (n_obs × n_obs) - covariance of observations
    // =========================================================================
    gsl_matrix *K_obs = gsl_matrix_alloc(n_obs, n_obs);

    // Diagonal blocks
    if (n_obs_f > 0) {
        fill_covariance_block(K_obs, obs_offset_f, obs_offset_f,
                              obs->x_f, n_obs_f, GP_TYPE_F, obs->x_f, n_obs_f, GP_TYPE_F,
                              sigma2, ell, ell_p, period, kernel);
    }
    if (n_obs_g > 0) {
        fill_covariance_block(K_obs, obs_offset_g, obs_offset_g,
                              obs->x_g, n_obs_g, GP_TYPE_G, obs->x_g, n_obs_g, GP_TYPE_G,
                              sigma2, ell, ell_p, period, kernel);
    }
    if (n_obs_h > 0) {
        fill_covariance_block(K_obs, obs_offset_h, obs_offset_h,
                              obs->x_h, n_obs_h, GP_TYPE_H, obs->x_h, n_obs_h, GP_TYPE_H,
                              sigma2, ell, ell_p, period, kernel);
    }

    // Off-diagonal blocks (symmetric)
    if (n_obs_f > 0 && n_obs_g > 0) {
        fill_covariance_block(K_obs, obs_offset_f, obs_offset_g,
                              obs->x_f, n_obs_f, GP_TYPE_F, obs->x_g, n_obs_g, GP_TYPE_G,
                              sigma2, ell, ell_p, period, kernel);
        fill_covariance_block(K_obs, obs_offset_g, obs_offset_f,
                              obs->x_g, n_obs_g, GP_TYPE_G, obs->x_f, n_obs_f, GP_TYPE_F,
                              sigma2, ell, ell_p, period, kernel);
    }
    if (n_obs_f > 0 && n_obs_h > 0) {
        fill_covariance_block(K_obs, obs_offset_f, obs_offset_h,
                              obs->x_f, n_obs_f, GP_TYPE_F, obs->x_h, n_obs_h, GP_TYPE_H,
                              sigma2, ell, ell_p, period, kernel);
        fill_covariance_block(K_obs, obs_offset_h, obs_offset_f,
                              obs->x_h, n_obs_h, GP_TYPE_H, obs->x_f, n_obs_f, GP_TYPE_F,
                              sigma2, ell, ell_p, period, kernel);
    }
    if (n_obs_g > 0 && n_obs_h > 0) {
        fill_covariance_block(K_obs, obs_offset_g, obs_offset_h,
                              obs->x_g, n_obs_g, GP_TYPE_G, obs->x_h, n_obs_h, GP_TYPE_H,
                              sigma2, ell, ell_p, period, kernel);
        fill_covariance_block(K_obs, obs_offset_h, obs_offset_g,
                              obs->x_h, n_obs_h, GP_TYPE_H, obs->x_g, n_obs_g, GP_TYPE_G,
                              sigma2, ell, ell_p, period, kernel);
    }

    // u observation blocks
    if (n_obs_u > 0) {
        fill_covariance_block(K_obs, obs_offset_u, obs_offset_u,
                              obs->x_u, n_obs_u, GP_TYPE_U, obs->x_u, n_obs_u, GP_TYPE_U,
                              sigma2, ell, ell_p, period, kernel);
    }
    if (n_obs_f > 0 && n_obs_u > 0) {
        fill_covariance_block(K_obs, obs_offset_f, obs_offset_u,
                              obs->x_f, n_obs_f, GP_TYPE_F, obs->x_u, n_obs_u, GP_TYPE_U,
                              sigma2, ell, ell_p, period, kernel);
        fill_covariance_block(K_obs, obs_offset_u, obs_offset_f,
                              obs->x_u, n_obs_u, GP_TYPE_U, obs->x_f, n_obs_f, GP_TYPE_F,
                              sigma2, ell, ell_p, period, kernel);
    }
    if (n_obs_g > 0 && n_obs_u > 0) {
        fill_covariance_block(K_obs, obs_offset_g, obs_offset_u,
                              obs->x_g, n_obs_g, GP_TYPE_G, obs->x_u, n_obs_u, GP_TYPE_U,
                              sigma2, ell, ell_p, period, kernel);
        fill_covariance_block(K_obs, obs_offset_u, obs_offset_g,
                              obs->x_u, n_obs_u, GP_TYPE_U, obs->x_g, n_obs_g, GP_TYPE_G,
                              sigma2, ell, ell_p, period, kernel);
    }
    if (n_obs_h > 0 && n_obs_u > 0) {
        fill_covariance_block(K_obs, obs_offset_h, obs_offset_u,
                              obs->x_h, n_obs_h, GP_TYPE_H, obs->x_u, n_obs_u, GP_TYPE_U,
                              sigma2, ell, ell_p, period, kernel);
        fill_covariance_block(K_obs, obs_offset_u, obs_offset_h,
                              obs->x_u, n_obs_u, GP_TYPE_U, obs->x_h, n_obs_h, GP_TYPE_H,
                              sigma2, ell, ell_p, period, kernel);
    }

    // Add observation noise covariance (supports scalar, diagonal, or full covariance)
    add_noise_covariance(K_obs, obs_offset_f, obs->noise_f_cov, obs->noise_f_type, n_obs_f);
    add_noise_covariance(K_obs, obs_offset_g, obs->noise_g_cov, obs->noise_g_type, n_obs_g);
    add_noise_covariance(K_obs, obs_offset_h, obs->noise_h_cov, obs->noise_h_type, n_obs_h);
    add_noise_covariance(K_obs, obs_offset_u, obs->noise_u_cov, obs->noise_u_type, n_obs_u);

    // =========================================================================
    // 2. Build K_pred (n_pred × n_pred) - prior covariance of predictions
    // =========================================================================
    gsl_matrix *K_pred = gsl_matrix_alloc(n_pred, n_pred);

    // Diagonal blocks
    if (n_pred_f > 0) {
        fill_covariance_block(K_pred, pred_offset_f, pred_offset_f,
                              pred->x_f, n_pred_f, GP_TYPE_F, pred->x_f, n_pred_f, GP_TYPE_F,
                              sigma2, ell, ell_p, period, kernel);
    }
    if (n_pred_g > 0) {
        fill_covariance_block(K_pred, pred_offset_g, pred_offset_g,
                              pred->x_g, n_pred_g, GP_TYPE_G, pred->x_g, n_pred_g, GP_TYPE_G,
                              sigma2, ell, ell_p, period, kernel);
    }
    if (n_pred_h > 0) {
        fill_covariance_block(K_pred, pred_offset_h, pred_offset_h,
                              pred->x_h, n_pred_h, GP_TYPE_H, pred->x_h, n_pred_h, GP_TYPE_H,
                              sigma2, ell, ell_p, period, kernel);
    }

    // Off-diagonal blocks
    if (n_pred_f > 0 && n_pred_g > 0) {
        fill_covariance_block(K_pred, pred_offset_f, pred_offset_g,
                              pred->x_f, n_pred_f, GP_TYPE_F, pred->x_g, n_pred_g, GP_TYPE_G,
                              sigma2, ell, ell_p, period, kernel);
        fill_covariance_block(K_pred, pred_offset_g, pred_offset_f,
                              pred->x_g, n_pred_g, GP_TYPE_G, pred->x_f, n_pred_f, GP_TYPE_F,
                              sigma2, ell, ell_p, period, kernel);
    }
    if (n_pred_f > 0 && n_pred_h > 0) {
        fill_covariance_block(K_pred, pred_offset_f, pred_offset_h,
                              pred->x_f, n_pred_f, GP_TYPE_F, pred->x_h, n_pred_h, GP_TYPE_H,
                              sigma2, ell, ell_p, period, kernel);
        fill_covariance_block(K_pred, pred_offset_h, pred_offset_f,
                              pred->x_h, n_pred_h, GP_TYPE_H, pred->x_f, n_pred_f, GP_TYPE_F,
                              sigma2, ell, ell_p, period, kernel);
    }
    if (n_pred_g > 0 && n_pred_h > 0) {
        fill_covariance_block(K_pred, pred_offset_g, pred_offset_h,
                              pred->x_g, n_pred_g, GP_TYPE_G, pred->x_h, n_pred_h, GP_TYPE_H,
                              sigma2, ell, ell_p, period, kernel);
        fill_covariance_block(K_pred, pred_offset_h, pred_offset_g,
                              pred->x_h, n_pred_h, GP_TYPE_H, pred->x_g, n_pred_g, GP_TYPE_G,
                              sigma2, ell, ell_p, period, kernel);
    }
    if (n_pred_u > 0) {
        fill_covariance_block(K_pred, pred_offset_u, pred_offset_u,
                              pred->x_u, n_pred_u, GP_TYPE_U, pred->x_u, n_pred_u, GP_TYPE_U,
                              sigma2, ell, ell_p, period, kernel);
    }
    if (n_pred_f > 0 && n_pred_u > 0) {
        fill_covariance_block(K_pred, pred_offset_f, pred_offset_u,
                              pred->x_f, n_pred_f, GP_TYPE_F, pred->x_u, n_pred_u, GP_TYPE_U,
                              sigma2, ell, ell_p, period, kernel);
        fill_covariance_block(K_pred, pred_offset_u, pred_offset_f,
                              pred->x_u, n_pred_u, GP_TYPE_U, pred->x_f, n_pred_f, GP_TYPE_F,
                              sigma2, ell, ell_p, period, kernel);
    }
    if (n_pred_g > 0 && n_pred_u > 0) {
        fill_covariance_block(K_pred, pred_offset_g, pred_offset_u,
                              pred->x_g, n_pred_g, GP_TYPE_G, pred->x_u, n_pred_u, GP_TYPE_U,
                              sigma2, ell, ell_p, period, kernel);
        fill_covariance_block(K_pred, pred_offset_u, pred_offset_g,
                              pred->x_u, n_pred_u, GP_TYPE_U, pred->x_g, n_pred_g, GP_TYPE_G,
                              sigma2, ell, ell_p, period, kernel);
    }
    if (n_pred_h > 0 && n_pred_u > 0) {
        fill_covariance_block(K_pred, pred_offset_h, pred_offset_u,
                              pred->x_h, n_pred_h, GP_TYPE_H, pred->x_u, n_pred_u, GP_TYPE_U,
                              sigma2, ell, ell_p, period, kernel);
        fill_covariance_block(K_pred, pred_offset_u, pred_offset_h,
                              pred->x_u, n_pred_u, GP_TYPE_U, pred->x_h, n_pred_h, GP_TYPE_H,
                              sigma2, ell, ell_p, period, kernel);
    }

    // =========================================================================
    // 3. Build K_pred_obs (n_pred × n_obs) - cross-covariance
    // =========================================================================
    gsl_matrix *K_pred_obs = gsl_matrix_alloc(n_pred, n_obs);

    // All 9 possible blocks
    if (n_pred_f > 0 && n_obs_f > 0) {
        fill_covariance_block(K_pred_obs, pred_offset_f, obs_offset_f,
                              pred->x_f, n_pred_f, GP_TYPE_F, obs->x_f, n_obs_f, GP_TYPE_F,
                              sigma2, ell, ell_p, period, kernel);
    }
    if (n_pred_f > 0 && n_obs_g > 0) {
        fill_covariance_block(K_pred_obs, pred_offset_f, obs_offset_g,
                              pred->x_f, n_pred_f, GP_TYPE_F, obs->x_g, n_obs_g, GP_TYPE_G,
                              sigma2, ell, ell_p, period, kernel);
    }
    if (n_pred_f > 0 && n_obs_h > 0) {
        fill_covariance_block(K_pred_obs, pred_offset_f, obs_offset_h,
                              pred->x_f, n_pred_f, GP_TYPE_F, obs->x_h, n_obs_h, GP_TYPE_H,
                              sigma2, ell, ell_p, period, kernel);
    }
    if (n_pred_g > 0 && n_obs_f > 0) {
        fill_covariance_block(K_pred_obs, pred_offset_g, obs_offset_f,
                              pred->x_g, n_pred_g, GP_TYPE_G, obs->x_f, n_obs_f, GP_TYPE_F,
                              sigma2, ell, ell_p, period, kernel);
    }
    if (n_pred_g > 0 && n_obs_g > 0) {
        fill_covariance_block(K_pred_obs, pred_offset_g, obs_offset_g,
                              pred->x_g, n_pred_g, GP_TYPE_G, obs->x_g, n_obs_g, GP_TYPE_G,
                              sigma2, ell, ell_p, period, kernel);
    }
    if (n_pred_g > 0 && n_obs_h > 0) {
        fill_covariance_block(K_pred_obs, pred_offset_g, obs_offset_h,
                              pred->x_g, n_pred_g, GP_TYPE_G, obs->x_h, n_obs_h, GP_TYPE_H,
                              sigma2, ell, ell_p, period, kernel);
    }
    if (n_pred_h > 0 && n_obs_f > 0) {
        fill_covariance_block(K_pred_obs, pred_offset_h, obs_offset_f,
                              pred->x_h, n_pred_h, GP_TYPE_H, obs->x_f, n_obs_f, GP_TYPE_F,
                              sigma2, ell, ell_p, period, kernel);
    }
    if (n_pred_h > 0 && n_obs_g > 0) {
        fill_covariance_block(K_pred_obs, pred_offset_h, obs_offset_g,
                              pred->x_h, n_pred_h, GP_TYPE_H, obs->x_g, n_obs_g, GP_TYPE_G,
                              sigma2, ell, ell_p, period, kernel);
    }
    if (n_pred_h > 0 && n_obs_h > 0) {
        fill_covariance_block(K_pred_obs, pred_offset_h, obs_offset_h,
                              pred->x_h, n_pred_h, GP_TYPE_H, obs->x_h, n_obs_h, GP_TYPE_H,
                              sigma2, ell, ell_p, period, kernel);
    }
    // u prediction vs all observation types
    if (n_pred_u > 0 && n_obs_f > 0) {
        fill_covariance_block(K_pred_obs, pred_offset_u, obs_offset_f,
                              pred->x_u, n_pred_u, GP_TYPE_U, obs->x_f, n_obs_f, GP_TYPE_F,
                              sigma2, ell, ell_p, period, kernel);
    }
    if (n_pred_u > 0 && n_obs_g > 0) {
        fill_covariance_block(K_pred_obs, pred_offset_u, obs_offset_g,
                              pred->x_u, n_pred_u, GP_TYPE_U, obs->x_g, n_obs_g, GP_TYPE_G,
                              sigma2, ell, ell_p, period, kernel);
    }
    if (n_pred_u > 0 && n_obs_h > 0) {
        fill_covariance_block(K_pred_obs, pred_offset_u, obs_offset_h,
                              pred->x_u, n_pred_u, GP_TYPE_U, obs->x_h, n_obs_h, GP_TYPE_H,
                              sigma2, ell, ell_p, period, kernel);
    }
    if (n_pred_u > 0 && n_obs_u > 0) {
        fill_covariance_block(K_pred_obs, pred_offset_u, obs_offset_u,
                              pred->x_u, n_pred_u, GP_TYPE_U, obs->x_u, n_obs_u, GP_TYPE_U,
                              sigma2, ell, ell_p, period, kernel);
    }
    // non-u predictions vs u observations
    if (n_pred_f > 0 && n_obs_u > 0) {
        fill_covariance_block(K_pred_obs, pred_offset_f, obs_offset_u,
                              pred->x_f, n_pred_f, GP_TYPE_F, obs->x_u, n_obs_u, GP_TYPE_U,
                              sigma2, ell, ell_p, period, kernel);
    }
    if (n_pred_g > 0 && n_obs_u > 0) {
        fill_covariance_block(K_pred_obs, pred_offset_g, obs_offset_u,
                              pred->x_g, n_pred_g, GP_TYPE_G, obs->x_u, n_obs_u, GP_TYPE_U,
                              sigma2, ell, ell_p, period, kernel);
    }
    if (n_pred_h > 0 && n_obs_u > 0) {
        fill_covariance_block(K_pred_obs, pred_offset_h, obs_offset_u,
                              pred->x_h, n_pred_h, GP_TYPE_H, obs->x_u, n_obs_u, GP_TYPE_U,
                              sigma2, ell, ell_p, period, kernel);
    }

    // =========================================================================
    // 4. Concatenate observation values into y_obs vector and subtract mean
    // =========================================================================
    gsl_vector *y_obs = gsl_vector_alloc(n_obs);
    int idx = 0;
    // f observations: subtract mean
    for (int i = 0; i < n_obs_f; i++) {
        double y = obs->y_f[i];
        double m = eval_mean_f(obs->x_f[i], mean);
        gsl_vector_set(y_obs, idx++, y - m);
    }
    // g observations: subtract mean
    for (int i = 0; i < n_obs_g; i++) {
        double y = obs->y_g[i];
        double m = eval_mean_g(obs->x_g[i], mean);
        gsl_vector_set(y_obs, idx++, y - m);
    }
    // h observations: subtract mean
    for (int i = 0; i < n_obs_h; i++) {
        double y = obs->y_h[i];
        double m = eval_mean_h(obs->x_h[i], mean);
        gsl_vector_set(y_obs, idx++, y - m);
    }
    // u observations: mean is always 0
    for (int i = 0; i < n_obs_u; i++) {
        gsl_vector_set(y_obs, idx++, obs->y_u[i]);
    }

    // =========================================================================
    // 5. Cholesky decompose K_obs with fallback
    // =========================================================================
    gsl_matrix *K_obs_copy = gsl_matrix_alloc(n_obs, n_obs);
    gsl_matrix_memcpy(K_obs_copy, K_obs);

    int chol_status = cholesky_with_fallback(K_obs, K_obs_copy, n_obs);
    gsl_matrix_free(K_obs_copy);
    if (chol_status != 0) {
        fprintf(stderr, "Cholesky decomposition of K_obs failed\n");
        gsl_matrix_free(K_obs);
        gsl_matrix_free(K_pred);
        gsl_matrix_free(K_pred_obs);
        gsl_vector_free(y_obs);
        gsl_rng_free(rng);
        return -3;
    }

    // =========================================================================
    // 6. Solve for alpha = K_obs^{-1} @ y_obs
    // =========================================================================
    gsl_vector *alpha = gsl_vector_alloc(n_obs);
    gsl_vector_memcpy(alpha, y_obs);
    gsl_linalg_cholesky_svx(K_obs, alpha);  // alpha = K_obs^{-1} @ y_obs

    // =========================================================================
    // 7. Compute posterior mean: mu_post = K_pred_obs @ alpha, then add m(x_pred)
    // =========================================================================
    gsl_vector *mu_post = gsl_vector_alloc(n_pred);
    gsl_blas_dgemv(CblasNoTrans, 1.0, K_pred_obs, alpha, 0.0, mu_post);

    // Copy posterior means to output arrays and add mean function
    if (f_mean != NULL && n_pred_f > 0) {
        for (int i = 0; i < n_pred_f; i++) {
            double centered = gsl_vector_get(mu_post, pred_offset_f + i);
            double m = eval_mean_f(pred->x_f[i], mean);
            f_mean[i] = centered + m;
        }
    }
    if (g_mean != NULL && n_pred_g > 0) {
        for (int i = 0; i < n_pred_g; i++) {
            double centered = gsl_vector_get(mu_post, pred_offset_g + i);
            double m = eval_mean_g(pred->x_g[i], mean);
            g_mean[i] = centered + m;
        }
    }
    if (h_mean != NULL && n_pred_h > 0) {
        for (int i = 0; i < n_pred_h; i++) {
            double centered = gsl_vector_get(mu_post, pred_offset_h + i);
            double m = eval_mean_h(pred->x_h[i], mean);
            h_mean[i] = centered + m;
        }
    }
    if (u_mean != NULL && n_pred_u > 0) {
        for (int i = 0; i < n_pred_u; i++) {
            u_mean[i] = gsl_vector_get(mu_post, pred_offset_u + i);
            // mean of u is always 0
        }
    }

    // =========================================================================
    // 8. Compute posterior covariance: K_post = K_pred - K_pred_obs @ K_obs^{-1} @ K_pred_obs^T
    // =========================================================================
    // First solve v = L^{-1} @ K_pred_obs^T where L is lower Cholesky of K_obs
    gsl_matrix *v = gsl_matrix_alloc(n_obs, n_pred);
    for (int j = 0; j < n_pred; j++) {
        for (int i = 0; i < n_obs; i++) {
            gsl_matrix_set(v, i, j, gsl_matrix_get(K_pred_obs, j, i));
        }
    }
    // Solve L @ v = K_pred_obs^T column by column
    for (int j = 0; j < n_pred; j++) {
        gsl_vector_view col = gsl_matrix_column(v, j);
        gsl_blas_dtrsv(CblasLower, CblasNoTrans, CblasNonUnit, K_obs, &col.vector);
    }

    // K_post = K_pred - v^T @ v
    gsl_matrix *K_post = gsl_matrix_alloc(n_pred, n_pred);
    gsl_matrix_memcpy(K_post, K_pred);
    gsl_blas_dgemm(CblasTrans, CblasNoTrans, -1.0, v, v, 1.0, K_post);

    // =========================================================================
    // 9. Extract posterior standard deviations from K_post diagonal (before jitter)
    // =========================================================================
    if (f_std != NULL && n_pred_f > 0) {
        for (int i = 0; i < n_pred_f; i++) {
            double var = gsl_matrix_get(K_post, pred_offset_f + i, pred_offset_f + i);
            f_std[i] = sqrt(fmax(var, 0.0));  // Clamp to avoid sqrt of negative due to numerical error
        }
    }
    if (g_std != NULL && n_pred_g > 0) {
        for (int i = 0; i < n_pred_g; i++) {
            double var = gsl_matrix_get(K_post, pred_offset_g + i, pred_offset_g + i);
            g_std[i] = sqrt(fmax(var, 0.0));
        }
    }
    if (h_std != NULL && n_pred_h > 0) {
        for (int i = 0; i < n_pred_h; i++) {
            double var = gsl_matrix_get(K_post, pred_offset_h + i, pred_offset_h + i);
            h_std[i] = sqrt(fmax(var, 0.0));
        }
    }
    if (u_std != NULL && n_pred_u > 0) {
        for (int i = 0; i < n_pred_u; i++) {
            double var = gsl_matrix_get(K_post, pred_offset_u + i, pred_offset_u + i);
            u_std[i] = sqrt(fmax(var, 0.0));
        }
    }

    // Add scale-aware jitter for numerical stability
    //
    // Problem: When ell is large, the covariance matrices become ill-conditioned:
    // - K_ff approaches σ² × 11^T (rank-1, condition number → ∞)
    // - K_hh approaches (σ²/ℓ²) × 11^T (also rank-1)
    // - K_post = K_pred - v^T @ v suffers from catastrophic cancellation
    //
    // The Cholesky decomposition of such matrices amplifies numerical errors,
    // causing the samples to have spurious high-frequency noise.
    //
    // Solution: Add jitter proportional to the SCALE of each block (max diagonal).
    // This ensures numerical stability by bounding the condition number.
    // Using jitter = epsilon × max_diagonal gives condition number ≤ 1/epsilon.
    //
    double regularization = 1e-6;  // Target condition number ~ 1e6
    double min_jitter = 1e-14;     // Absolute minimum for numerical safety

    // Find max diagonal for each block to determine scale
    double max_diag_f = 0.0, max_diag_g = 0.0, max_diag_h = 0.0, max_diag_u = 0.0;

    for (int i = 0; i < n_pred_f; i++) {
        double val = gsl_matrix_get(K_pred, pred_offset_f + i, pred_offset_f + i);
        if (val > max_diag_f) max_diag_f = val;
    }
    for (int i = 0; i < n_pred_g; i++) {
        double val = gsl_matrix_get(K_pred, pred_offset_g + i, pred_offset_g + i);
        if (val > max_diag_g) max_diag_g = val;
    }
    for (int i = 0; i < n_pred_h; i++) {
        double val = gsl_matrix_get(K_pred, pred_offset_h + i, pred_offset_h + i);
        if (val > max_diag_h) max_diag_h = val;
    }
    for (int i = 0; i < n_pred_u; i++) {
        double val = gsl_matrix_get(K_pred, pred_offset_u + i, pred_offset_u + i);
        if (val > max_diag_u) max_diag_u = val;
    }

    // Compute block-specific jitter based on scale
    double jitter_f = fmax(regularization * max_diag_f, min_jitter);
    double jitter_g = fmax(regularization * max_diag_g, min_jitter);
    double jitter_h = fmax(regularization * max_diag_h, min_jitter);
    double jitter_u = fmax(regularization * max_diag_u, min_jitter);

    // Apply jitter to each block, ensuring non-negative diagonal
    if (n_pred_f > 0) {
        for (int i = 0; i < n_pred_f; i++) {
            double val = gsl_matrix_get(K_post, pred_offset_f + i, pred_offset_f + i);
            gsl_matrix_set(K_post, pred_offset_f + i, pred_offset_f + i, fmax(val, 0.0) + jitter_f);
        }
    }
    if (n_pred_g > 0) {
        for (int i = 0; i < n_pred_g; i++) {
            double val = gsl_matrix_get(K_post, pred_offset_g + i, pred_offset_g + i);
            gsl_matrix_set(K_post, pred_offset_g + i, pred_offset_g + i, fmax(val, 0.0) + jitter_g);
        }
    }
    if (n_pred_h > 0) {
        for (int i = 0; i < n_pred_h; i++) {
            double val = gsl_matrix_get(K_post, pred_offset_h + i, pred_offset_h + i);
            gsl_matrix_set(K_post, pred_offset_h + i, pred_offset_h + i, fmax(val, 0.0) + jitter_h);
        }
    }
    if (n_pred_u > 0) {
        for (int i = 0; i < n_pred_u; i++) {
            double val = gsl_matrix_get(K_post, pred_offset_u + i, pred_offset_u + i);
            gsl_matrix_set(K_post, pred_offset_u + i, pred_offset_u + i, fmax(val, 0.0) + jitter_u);
        }
    }

    // =========================================================================
    // 10. Cholesky decompose K_post with fallback and sample
    // =========================================================================
    gsl_matrix *K_post_copy = gsl_matrix_alloc(n_pred, n_pred);
    gsl_matrix_memcpy(K_post_copy, K_post);

    chol_status = cholesky_with_fallback(K_post, K_post_copy, n_pred);
    gsl_matrix_free(K_post_copy);
    if (chol_status != 0) {
        fprintf(stderr, "Cholesky decomposition of K_post failed\n");
        gsl_matrix_free(K_obs);
        gsl_matrix_free(K_pred);
        gsl_matrix_free(K_pred_obs);
        gsl_matrix_free(K_post);
        gsl_matrix_free(v);
        gsl_vector_free(y_obs);
        gsl_vector_free(alpha);
        gsl_vector_free(mu_post);
        gsl_rng_free(rng);
        return -4;
    }

    // Sample from posterior
    gsl_vector *z = gsl_vector_alloc(n_pred);
    gsl_vector *sample = gsl_vector_alloc(n_pred);

    for (int s = 0; s < n_samples; s++) {
        // Draw standard normal samples
        for (int i = 0; i < n_pred; i++) {
            gsl_vector_set(z, i, gsl_ran_gaussian(rng, 1.0));
        }

        // sample = mu_post + L_post @ z
        gsl_vector_memcpy(sample, z);
        gsl_blas_dtrmv(CblasLower, CblasNoTrans, CblasNonUnit, K_post, sample);
        gsl_vector_add(sample, mu_post);

        // Extract samples to output arrays and add mean function
        if (f_samples != NULL && n_pred_f > 0) {
            for (int i = 0; i < n_pred_f; i++) {
                double centered = gsl_vector_get(sample, pred_offset_f + i);
                double m = eval_mean_f(pred->x_f[i], mean);
                f_samples[s * n_pred_f + i] = centered + m;
            }
        }
        if (g_samples != NULL && n_pred_g > 0) {
            for (int i = 0; i < n_pred_g; i++) {
                double centered = gsl_vector_get(sample, pred_offset_g + i);
                double m = eval_mean_g(pred->x_g[i], mean);
                g_samples[s * n_pred_g + i] = centered + m;
            }
        }
        if (h_samples != NULL && n_pred_h > 0) {
            for (int i = 0; i < n_pred_h; i++) {
                double centered = gsl_vector_get(sample, pred_offset_h + i);
                double m = eval_mean_h(pred->x_h[i], mean);
                h_samples[s * n_pred_h + i] = centered + m;
            }
        }
        if (u_samples != NULL && n_pred_u > 0) {
            for (int i = 0; i < n_pred_u; i++) {
                u_samples[s * n_pred_u + i] = gsl_vector_get(sample, pred_offset_u + i);
                // mean of u is always 0
            }
        }
    }

    // Cleanup
    gsl_matrix_free(K_obs);
    gsl_matrix_free(K_pred);
    gsl_matrix_free(K_pred_obs);
    gsl_matrix_free(K_post);
    gsl_matrix_free(v);
    gsl_vector_free(y_obs);
    gsl_vector_free(alpha);
    gsl_vector_free(mu_post);
    gsl_vector_free(z);
    gsl_vector_free(sample);
    gsl_rng_free(rng);

    return 0;
}


/* ============================================================================
 * Log Marginal Likelihood Computation
 * ============================================================================
 * Computes: log p(y|X,θ) = -0.5 * y^T K^{-1} y - 0.5 * log|K| - n/2 * log(2π)
 *
 * This is used for hyperparameter optimization.
 */

int compute_log_marginal_likelihood(
    const GPObservations *obs,
    double sigma2, double ell, double ell_p, double period,
    KernelType kernel,
    double *log_likelihood,
    const GPMeanSpec *mean
) {
    // Get observation counts
    int n_obs_f = (obs->x_f != NULL && obs->y_f != NULL) ? obs->n_f : 0;
    int n_obs_g = (obs->x_g != NULL && obs->y_g != NULL) ? obs->n_g : 0;
    int n_obs_h = (obs->x_h != NULL && obs->y_h != NULL) ? obs->n_h : 0;
    int n_obs_u = (obs->x_u != NULL && obs->y_u != NULL) ? obs->n_u : 0;
    int n_obs = n_obs_f + n_obs_g + n_obs_h + n_obs_u;

    if (n_obs == 0) {
        fprintf(stderr, "No observations provided\n");
        return -1;
    }

    // Validate: periodic kernels do NOT support integral observations
    if (kernel == KERNEL_PERIODIC || kernel == KERNEL_LOCALLY_PERIODIC) {
        if (n_obs_g > 0) {
            fprintf(stderr, "ERROR: Periodic kernels do not support integral observations (x_g).\n");
            return -1;
        }
    }

    // Offsets for observation blocks
    int obs_offset_f = 0;
    int obs_offset_g = n_obs_f;
    int obs_offset_h = n_obs_f + n_obs_g;
    int obs_offset_u = n_obs_f + n_obs_g + n_obs_h;

    // =========================================================================
    // 1. Build K_obs (n_obs × n_obs) - covariance of observations
    // =========================================================================
    gsl_matrix *K_obs = gsl_matrix_alloc(n_obs, n_obs);

    // Diagonal blocks
    if (n_obs_f > 0) {
        fill_covariance_block(K_obs, obs_offset_f, obs_offset_f,
                              obs->x_f, n_obs_f, GP_TYPE_F, obs->x_f, n_obs_f, GP_TYPE_F,
                              sigma2, ell, ell_p, period, kernel);
    }
    if (n_obs_g > 0) {
        fill_covariance_block(K_obs, obs_offset_g, obs_offset_g,
                              obs->x_g, n_obs_g, GP_TYPE_G, obs->x_g, n_obs_g, GP_TYPE_G,
                              sigma2, ell, ell_p, period, kernel);
    }
    if (n_obs_h > 0) {
        fill_covariance_block(K_obs, obs_offset_h, obs_offset_h,
                              obs->x_h, n_obs_h, GP_TYPE_H, obs->x_h, n_obs_h, GP_TYPE_H,
                              sigma2, ell, ell_p, period, kernel);
    }

    // Off-diagonal blocks (symmetric)
    if (n_obs_f > 0 && n_obs_g > 0) {
        fill_covariance_block(K_obs, obs_offset_f, obs_offset_g,
                              obs->x_f, n_obs_f, GP_TYPE_F, obs->x_g, n_obs_g, GP_TYPE_G,
                              sigma2, ell, ell_p, period, kernel);
        fill_covariance_block(K_obs, obs_offset_g, obs_offset_f,
                              obs->x_g, n_obs_g, GP_TYPE_G, obs->x_f, n_obs_f, GP_TYPE_F,
                              sigma2, ell, ell_p, period, kernel);
    }
    if (n_obs_f > 0 && n_obs_h > 0) {
        fill_covariance_block(K_obs, obs_offset_f, obs_offset_h,
                              obs->x_f, n_obs_f, GP_TYPE_F, obs->x_h, n_obs_h, GP_TYPE_H,
                              sigma2, ell, ell_p, period, kernel);
        fill_covariance_block(K_obs, obs_offset_h, obs_offset_f,
                              obs->x_h, n_obs_h, GP_TYPE_H, obs->x_f, n_obs_f, GP_TYPE_F,
                              sigma2, ell, ell_p, period, kernel);
    }
    if (n_obs_g > 0 && n_obs_h > 0) {
        fill_covariance_block(K_obs, obs_offset_g, obs_offset_h,
                              obs->x_g, n_obs_g, GP_TYPE_G, obs->x_h, n_obs_h, GP_TYPE_H,
                              sigma2, ell, ell_p, period, kernel);
        fill_covariance_block(K_obs, obs_offset_h, obs_offset_g,
                              obs->x_h, n_obs_h, GP_TYPE_H, obs->x_g, n_obs_g, GP_TYPE_G,
                              sigma2, ell, ell_p, period, kernel);
    }

    if (n_obs_u > 0) {
        fill_covariance_block(K_obs, obs_offset_u, obs_offset_u,
                              obs->x_u, n_obs_u, GP_TYPE_U, obs->x_u, n_obs_u, GP_TYPE_U,
                              sigma2, ell, ell_p, period, kernel);
    }
    if (n_obs_f > 0 && n_obs_u > 0) {
        fill_covariance_block(K_obs, obs_offset_f, obs_offset_u,
                              obs->x_f, n_obs_f, GP_TYPE_F, obs->x_u, n_obs_u, GP_TYPE_U,
                              sigma2, ell, ell_p, period, kernel);
        fill_covariance_block(K_obs, obs_offset_u, obs_offset_f,
                              obs->x_u, n_obs_u, GP_TYPE_U, obs->x_f, n_obs_f, GP_TYPE_F,
                              sigma2, ell, ell_p, period, kernel);
    }
    if (n_obs_g > 0 && n_obs_u > 0) {
        fill_covariance_block(K_obs, obs_offset_g, obs_offset_u,
                              obs->x_g, n_obs_g, GP_TYPE_G, obs->x_u, n_obs_u, GP_TYPE_U,
                              sigma2, ell, ell_p, period, kernel);
        fill_covariance_block(K_obs, obs_offset_u, obs_offset_g,
                              obs->x_u, n_obs_u, GP_TYPE_U, obs->x_g, n_obs_g, GP_TYPE_G,
                              sigma2, ell, ell_p, period, kernel);
    }
    if (n_obs_h > 0 && n_obs_u > 0) {
        fill_covariance_block(K_obs, obs_offset_h, obs_offset_u,
                              obs->x_h, n_obs_h, GP_TYPE_H, obs->x_u, n_obs_u, GP_TYPE_U,
                              sigma2, ell, ell_p, period, kernel);
        fill_covariance_block(K_obs, obs_offset_u, obs_offset_h,
                              obs->x_u, n_obs_u, GP_TYPE_U, obs->x_h, n_obs_h, GP_TYPE_H,
                              sigma2, ell, ell_p, period, kernel);
    }

    // Add observation noise covariance
    add_noise_covariance(K_obs, obs_offset_f, obs->noise_f_cov, obs->noise_f_type, n_obs_f);
    add_noise_covariance(K_obs, obs_offset_g, obs->noise_g_cov, obs->noise_g_type, n_obs_g);
    add_noise_covariance(K_obs, obs_offset_h, obs->noise_h_cov, obs->noise_h_type, n_obs_h);
    add_noise_covariance(K_obs, obs_offset_u, obs->noise_u_cov, obs->noise_u_type, n_obs_u);

    // =========================================================================
    // 2. Build y_obs vector (subtract mean if specified)
    // =========================================================================
    gsl_vector *y_obs = gsl_vector_alloc(n_obs);
    int idx = 0;
    for (int i = 0; i < n_obs_f; i++) {
        double y = obs->y_f[i];
        double m = eval_mean_f(obs->x_f[i], mean);
        gsl_vector_set(y_obs, idx++, y - m);
    }
    for (int i = 0; i < n_obs_g; i++) {
        double y = obs->y_g[i];
        double m = eval_mean_g(obs->x_g[i], mean);
        gsl_vector_set(y_obs, idx++, y - m);
    }
    for (int i = 0; i < n_obs_h; i++) {
        double y = obs->y_h[i];
        double m = eval_mean_h(obs->x_h[i], mean);
        gsl_vector_set(y_obs, idx++, y - m);
    }
    for (int i = 0; i < n_obs_u; i++) {
        double y = obs->y_u[i];
        double m = eval_mean_u(obs->x_u[i], mean);
        gsl_vector_set(y_obs, idx++, y - m);
    }

    // =========================================================================
    // 3. Cholesky decompose K_obs with fallback
    // =========================================================================
    gsl_matrix *K_obs_copy = gsl_matrix_alloc(n_obs, n_obs);
    gsl_matrix_memcpy(K_obs_copy, K_obs);

    int chol_status = cholesky_with_fallback(K_obs, K_obs_copy, n_obs);
    gsl_matrix_free(K_obs_copy);

    if (chol_status != 0) {
        // Cholesky failed - return very negative log likelihood
        *log_likelihood = -1e30;
        gsl_matrix_free(K_obs);
        gsl_vector_free(y_obs);
        return 0;  // Not an error - just a bad parameter region
    }

    // =========================================================================
    // 4. Compute log|K| = 2 * sum(log(diag(L)))
    // =========================================================================
    double log_det = 0.0;
    for (int i = 0; i < n_obs; i++) {
        double L_ii = gsl_matrix_get(K_obs, i, i);
        if (L_ii <= 0) {
            // Numerical issue - return very negative likelihood
            *log_likelihood = -1e30;
            gsl_matrix_free(K_obs);
            gsl_vector_free(y_obs);
            return 0;
        }
        log_det += log(L_ii);
    }
    log_det *= 2.0;  // log|K| = 2 * sum(log(diag(L)))

    // =========================================================================
    // 5. Solve for alpha = K_obs^{-1} @ y_obs
    // =========================================================================
    gsl_vector *alpha = gsl_vector_alloc(n_obs);
    gsl_vector_memcpy(alpha, y_obs);
    gsl_linalg_cholesky_svx(K_obs, alpha);  // alpha = K_obs^{-1} @ y_obs

    // =========================================================================
    // 6. Compute data fit term: y^T @ alpha
    // =========================================================================
    double data_fit = 0.0;
    for (int i = 0; i < n_obs; i++) {
        data_fit += gsl_vector_get(y_obs, i) * gsl_vector_get(alpha, i);
    }

    // =========================================================================
    // 7. Compute log marginal likelihood
    // =========================================================================
    // log p(y|X,θ) = -0.5 * y^T K^{-1} y - 0.5 * log|K| - n/2 * log(2π)
    double log_2pi = 1.8378770664093454835606594728112;  // log(2π)
    *log_likelihood = -0.5 * data_fit - 0.5 * log_det - 0.5 * n_obs * log_2pi;

    // Cleanup
    gsl_matrix_free(K_obs);
    gsl_vector_free(y_obs);
    gsl_vector_free(alpha);

    return 0;
}
