#include "header.h"




void TBD1d(double *ux, double *uy, double *sx, double *sy, double *phi, int *L, int *N, double *result){
  int l, j;
  double arg;
  int nloc = N[0];
  int Lfreq = L[0];

  for(l = 0; l < Lfreq; l++){
    for(j = 0; j < nloc; j++){
      arg = 2.0 * M_PI * (ux[l] * sx[j] + uy[l] * sy[j]) + phi[l];
      result[j] += 2.0 * cos(arg);
    }
  }
}

void TBD1d_amp(double * restrict ux, double * restrict uy,
               double * restrict sx, double * restrict sy,
               double * restrict phi, double * restrict amp,
               int *L, int *N,
               double * restrict result)
{
  int nloc  = N[0];
  int Lfreq = L[0];
  const double two_pi = 2.0 * M_PI;

  /* Precompute 2*pi*ux[l] and 2*pi*uy[l] once: these arrays have length
     Lfreq (typically 1e4-1e5, a few hundred KB) and are reused for every
     location below, so they stay resident in L2/L3 cache. This also
     removes two multiplications from the hot inner loop. */
  double *a = (double *) R_Calloc(Lfreq, double);
  double *b = (double *) R_Calloc(Lfreq, double);
  for (int l = 0; l < Lfreq; l++) {
    a[l] = two_pi * ux[l];
    b[l] = two_pi * uy[l];
  }

  /* Loop order: location (j) outer, frequency (l) inner.
     This is the key change vs. the original (l outer, j inner):
     with nloc possibly in the millions, looping l outer forced the
     CPU to stream the whole result/sx/sy arrays (several MB each)
     back and forth for every single frequency, which is bandwidth-
     bound and cache-hostile. With j outer, the small per-frequency
     arrays (a, b, amp, phi) are reused across all locations while
     sx[j]/sy[j] are read once and the partial sum is accumulated in
     a scalar register (acc), which the compiler can also auto-
     vectorize with -O3 since all pointers are restrict-qualified. */
  for (int j = 0; j < nloc; j++) {
    const double sxj = sx[j];
    const double syj = sy[j];
    double acc = 0.0;
    for (int l = 0; l < Lfreq; l++) {
      double arg = a[l] * sxj + b[l] * syj + phi[l];
      acc += amp[l] * cos(arg);
    }
    result[j] = acc;
  }

  R_Free(a);
  R_Free(b);
}


/* Evaluate the finite-L spectral turning-bands sum on two- or
   three-dimensional coordinates. Both omega and coords are supplied in
   R column-major order: omega is L x d and coords is N x d. */
void TBDnd_amp(double * restrict omega, double * restrict coords,
               double * restrict phi, double * restrict amp,
               int *L, int *N, int *d,
               double * restrict result)
{
  const int nloc = N[0];
  const int Lfreq = L[0];
  const int ndim = d[0];
  const double two_pi = 2.0 * M_PI;

  if (ndim != 2 && ndim != 3) {
    for (int j = 0; j < nloc; j++) result[j] = R_NaN;
    return;
  }

  const double *ux = omega;
  const double *uy = omega + Lfreq;
  const double *uz = (ndim == 3) ? omega + 2 * Lfreq : NULL;
  const double *sx = coords;
  const double *sy = coords + nloc;
  const double *sz = (ndim == 3) ? coords + 2 * nloc : NULL;

  double *a = (double *) R_Calloc(Lfreq, double);
  double *b = (double *) R_Calloc(Lfreq, double);
  double *c = (ndim == 3) ? (double *) R_Calloc(Lfreq, double) : NULL;

  for (int l = 0; l < Lfreq; l++) {
    a[l] = two_pi * ux[l];
    b[l] = two_pi * uy[l];
    if (ndim == 3) c[l] = two_pi * uz[l];
  }

  if (ndim == 2) {
    for (int j = 0; j < nloc; j++) {
      const double sxj = sx[j];
      const double syj = sy[j];
      double acc = 0.0;
      for (int l = 0; l < Lfreq; l++) {
        const double arg = a[l] * sxj + b[l] * syj + phi[l];
        acc += amp[l] * cos(arg);
      }
      result[j] = acc;
    }
  } else {
    for (int j = 0; j < nloc; j++) {
      const double sxj = sx[j];
      const double syj = sy[j];
      const double szj = sz[j];
      double acc = 0.0;
      for (int l = 0; l < Lfreq; l++) {
        const double arg = a[l] * sxj + b[l] * syj + c[l] * szj + phi[l];
        acc += amp[l] * cos(arg);
      }
      result[j] = acc;
    }
  }

  R_Free(a);
  R_Free(b);
  if (c != NULL) R_Free(c);
}



void hyperg_1F2_e_call( double *a,  double *b,double *c,  double *x, double *val)
{
    double tol = 1e-12;
    *val = hypergeometric_1F2(*a, *b,*c, *x,tol);


}


double hypergeometric_1F2(double a, double b, double c, double x, double tol) {
    int max_terms = 2000;
    double sum = 1.0;
    double term = 1.0;
    int n;

    // --- Parameter validity checks ---
    if ((b <= 0.0 && b == floor(b)) || 
        (c <= 0.0 && c == floor(c))) {
        return R_NaN;  // poles in denominator
    }

    // --- Case 1: a is a non-positive integer → finite polynomial ---
    if (a <= 0.0 && a == floor(a)) {
        int max_n = (int)(-a);
        for (n = 1; n <= max_n; n++) {
            term *= (a + n - 1) * x / ((b + n - 1) * (c + n - 1) * n);
            sum += term;
        }
        return sum;
    }

    // --- Case 2: General infinite series ---
    for (n = 1; n < max_terms; n++) {
        term *= (a + n - 1) * x / ((b + n - 1) * (c + n - 1) * n);

        if (!R_FINITE(term)) {
            return sum;  // bailout on overflow/NaN
        }

        sum += term;

        // convergence test: use user-provided tolerance
        if (fabs(term) < tol * fabs(sum)) {
            break;
        }
    }

    // If we hit max_terms without convergence, user should be warned.
    return sum;
}




/*******************************************************************/
double den_mat(double z,double k,double sc){  
  double u=2*M_PI;
z=z*u;
double res= exp(lgammafn(k+1)+2*log(sc)-(log(M_PI)+lgammafn(k)+(k+1)*log(1+R_pow(sc*z,2))));
return(res);
}



double ff(double lambda, double mu, double k, double zsc, double sc, double tol, int d) {
    double pi = M_PI;
    double two_pow = R_pow(2.0, 1.0 - 2.0 * k);
    double gamma_d_half = gammafn(d + 0.5);
    double rat = sqrt(pi) * two_pow / gamma_d_half;
    double pi_inv = 1.0 / pi;
    double gamma_mu_2k_1 = gammafn(mu + 2.0 * k + 1.0);
    double gamma_2k_d = gammafn(2.0 * k + d);
    double sc_pow_d = R_pow(sc, d);
    double numerator = rat * 0.25 * pi_inv * gamma_mu_2k_1 * gamma_2k_d * sc_pow_d;
    double lgamma_k_d2 = lgammafn(k + d / 2.0);
    double lgamma_mu_2k_d1 = lgammafn(mu + 2.0 * k + d + 1.0);
    double denominator = exp(lgamma_k_d2 + lgamma_mu_2k_d1);
    double arg = - (zsc * zsc) / 4.0;
    double hyperg = hypergeometric_1F2(lambda, lambda + 0.5 * mu, lambda + 0.5 * (mu + 1.0), arg, tol);
    double res = numerator * hyperg / denominator;
    return res;
}


double ff_hyp(double lambda,double mu,double k,double zsc,double sc,double tol, int d)
{
double a,b,c,res;
a= gammafn(lambda) * gammafn(lambda + 0.5*mu - d / 2) * gammafn(lambda + 0.5*mu+k) * R_pow(sc,d);
b=R_pow(2,d) * R_pow(M_PI,d / 2) * exp(lgammafn(lambda - d / 2) + lgammafn(lambda + 0.5*mu) + lgammafn(lambda + 0.5*mu+d/2+k));
c=hypergeometric_1F2(lambda, lambda + 0.5*mu, lambda + 0.5*mu+d/2+k, -R_pow(zsc,2)/4, tol);
res=a*c/b;
return(res);
}





/********************************************************************/
double den_wen_gen_mat(double z,double k,double sc,double mu,double tol){ 
double res;
int d=2;
double beta1=sc;
double rep=R_pow(gammafn(mu+2*k+1)/(gammafn(mu)),1/(1+2*k));
sc=sc*rep;
double lambda=1.5+k;
double u=2*M_PI;
z=z*u;
double zsc=z*sc;
double uff=ff(lambda, mu, k,zsc,sc,tol,d);
double mat=den_mat(z/u,k+0.5,beta1);
if(fabs(uff)<=mat) res=uff;
else res=mat;
return(res);
}
/********************************************************************/
double den_wen_gen_mat2(double z,double k,double sc,double mu,double tol){ 
double res;
int d=2;
double beta1=sc;
double rep=mu;
sc=sc*rep;
double lambda=1.5+k;
double u=2*M_PI;
z=z*u;
double zsc=z*sc;
double uff=ff(lambda, mu, k,zsc,sc,tol,d);
double mat=den_mat(z/u,k+0.5,beta1);
if(fabs(uff)<=mat) res=uff;
else res=mat;
return(res);
}
/********************************************************************/
double den_hyp_gen_mat(double z,double k,double sc,double mu,double tol){ 
double res;
int d=2;
double beta1=sc;
//double rep=R_pow(gammafn(mu+2*k+1)/(gammafn(mu)),1/(1+2*k));
double rep=R_pow(R_pow(2,2*k+1)*gammafn((mu+1)/2+k)*gammafn((mu+d+1)/2+2*k)/(gammafn(mu/2)*gammafn((mu+d)/2+k)),1/(1+2*k));
sc=sc*rep;
double lambda=1.5+k;
double u=2*M_PI;
z=z*u;
double zsc=z*sc;
double uff=ff_hyp(lambda, mu, k,zsc,sc,tol,d);
double mat=den_mat(z/u,k+0.5,beta1);
if(fabs(uff)<=mat) res=uff;
else res=mat;
return(res);
}
/********************************************************************/
double den_hyp_gen_mat2(double z,double k,double sc,double mu,double tol){ 
double res;
int d=2;
double beta1=sc;
//double rep=R_pow(gammafn(mu+2*k+1)/(gammafn(mu)),1/(1+2*k));
double rep=mu;
sc=sc*rep;
double lambda=1.5+k;
double u=2*M_PI;
z=z*u;
double zsc=z*sc;
double uff=ff_hyp(lambda, mu, k,zsc,sc,tol,d);
double mat=den_mat(z/u,k+0.5,beta1);
if(fabs(uff)<=mat) res=uff;
else res=mat;
return(res);
}



/*******************************************************************/
void spectraldensityC(double u, int model, int d, int L, double *f, double *av, double *Cv, double *nu1v, double *nu2v, double *params_other) {
    double norm_u = u;
    size_t n = (size_t)(L * L);

    // Allocazione vettore r con controllo
    double *r = (double *) R_Calloc(n, double);
    if (r == NULL) {
        // Gestione errore allocazione
        return;
    }

    double tol = 1e-12;

    // Calcolo densità spettrale in base al modello
    switch (model) {
        case 14: // Matern
            for (size_t i = 0; i < n; i++) {
                r[i] = den_mat(norm_u, nu1v[i], av[i]);
            }
            break;
            for (size_t i = 0; i < n; i++) {
                r[i] = den_wen_gen_mat(norm_u, nu1v[i], av[i], params_other[0], tol);
            }
            break;
        case 7: // GenWendland_matern2
            for (size_t i = 0; i < n; i++) {
                r[i] = den_wen_gen_mat2(norm_u, nu1v[i], av[i], params_other[0], tol);
            }
            break;
        case 23: // Hypergeometric_matern
            for (size_t i = 0; i < n; i++) {
                r[i] = den_hyp_gen_mat(norm_u, nu1v[i], av[i], params_other[0], tol);
            }
            break;
        case 30: // Hypergeometric_matern2
            for (size_t i = 0; i < n; i++) {
                r[i] = den_hyp_gen_mat2(norm_u, nu1v[i], av[i], params_other[0], tol);
            }
            break;
        default:
            // Modello non riconosciuto: azzera r
            for (size_t i = 0; i < n; i++) {
                r[i] = 0.0;
            }
            break;
    }
    for (size_t i = 0; i < n; i++) {
        f[i] = r[i];
    }

    R_Free(r);
}


// preguntar si las coordenadas son enteras
void extraer(double *coord, int sequen1, double *sub_coord,
             int fila, int col, int d) {
    /* coord is col x d and sub_coord is fila x d, both in R
       column-major order.  sequen1 is the zero-based first row of the
       requested block. */
    for (int j = 0; j < fila; j++) {
        const int source_row = sequen1 + j;
        for (int h = 0; h < d; h++) {
            sub_coord[j + h * fila] = coord[source_row + h * col];
        }
    }
}


void rellenar_indice(int *index,int inicio, int final,int largo){
    for (size_t i=0;i<largo;i++){index[i] = inicio+i;}
}


void u_index_extraer(double *u_index,double *u, int *index,int largo,int d,int filas){
    for (int i=0;i<largo;i++){
        for (int h=0;h<d;h++){
            u_index[i+h*filas] = u[index[i]+h*filas]; }}
}


void mult_mat(double *z, double *x, int xrows, int xcols, double *y, int yrows, int ycols){ 
  char *notrans = "N";
  double one = 1.0, zero = 0.0, *tmp = NULL;
  tmp = (double *) R_Calloc(xrows * ycols, double);
  F77_CALL(dgemm)(notrans, notrans, &xrows, &ycols, &xcols, &one, x, &xrows, y, &yrows, &zero, tmp, &xrows FCONE FCONE);
  Memcpy(z, tmp, xrows * ycols);
  R_Free(tmp);
}

void tcrossprod(double *z, double *x,  int xrows, int xcols, double *y, int yrows, int ycols){ /* outer product of two given matrices. z <- x %*% t(y) */
  char *notrans = "N", *trans = "T";
  double one = 1.0, zero = 0.0, *tmp = NULL;
  /* use tmp so z can be either x or y */
  tmp = (double *) R_Calloc(xrows * yrows, double);
  F77_CALL(dgemm)(notrans, trans, &xrows, &yrows, &xcols, &one, x, &xrows, y, &yrows, &zero, tmp, &xrows FCONE FCONE);
  Memcpy(z, tmp, xrows * yrows);
  R_Free(tmp);
}


void mult_x_cons(double *x, double cte,int largo){
  int i; 
  for (i=0;i<largo;i++){x[i] = cte*x[i];}
}

void sumar_matrices(double *x0, double *x,double *y,int largo){
  int h; 
  for (h=0;h<largo;h++){x0[h] = x[h]+y[h];}
}

void restar_matrices(double *x0, double *x,double *y,int largo){
    int h; 
    for (h=0;h<largo;h++){x0[h] = x[h]-y[h];}
}


void cos_vec(double *x_cos,double *x,int largo){
  int h;
  for (h=0;h<largo;h++){x_cos[h] = cos(x[h]);}
}

void sen_vec(double *x_sen,double *x,int largo){
    int h;
    for (h=0;h<largo;h++){x_sen[h] = sin(x[h]);}
}

void llenar_simu(double *x,double *simu, int N,int *P, int m){

    int j=0;
    for (int i = N* *(P)*m;i<(N+1)* *(P)*m;i++){simu[i] = x[j];j = j+1;}

}
void extraer_col(int inicio, int final,double *x_original,double *x){
    int j=0;
    for(int i=inicio;i<=final;i++){x[j] = x_original[i];j=j+1;}
}

void llenar_simu1(double *simu1, double *simu, int *m, int *P,
                  int *N, int lim, int block, double *L1) {
    /* Scatter a coordinate block into an n x P result for every
       realization.  Both local and destination arrays use R
       column-major order. */
    int total_n = 0;
    int start = 0;
    for (int b = 0; b < lim; b++) {
        if (b < block) start += m[b];
        total_n += m[b];
    }
    const int block_n = m[block];
    const double scale = 1.0 / sqrt(*L1);

    for (int k = 0; k < *N; k++) {
        for (int h = 0; h < *P; h++) {
            for (int j = 0; j < block_n; j++) {
                const int src = k * (*P) * block_n + h * block_n + j;
                const int dst = k * (*P) * total_n + h * total_n + start + j;
                simu1[dst] = simu[src] * scale;
            }
        }
    }
}


void C_tcrossprod(Rcomplex *z, Rcomplex *x,  int xrows, int xcols, Rcomplex *y, int yrows, int ycols){ /* outer product of two given matrices. z <- x %*% t(y) */
  char *notrans = "N", *trans = "T";
  Rcomplex one, zero, *tmp = NULL;
  one.r = 1;
  one.i = 0;
  zero.i =0;
  zero.r = 0;


      tmp = (Rcomplex *) R_Calloc(xrows * ycols, Rcomplex);
  F77_CALL(zgemm)(notrans, trans, &xrows, &yrows, &xcols, &one, x, &xrows, y, &yrows, &zero, tmp, &xrows FCONE FCONE);
  Memcpy(z, tmp, xrows * yrows);
  R_Free(tmp);
}


void C_mult_mat(Rcomplex *z, Rcomplex *x, int xrows, int xcols, Rcomplex *y, int yrows, int ycols){ 
  char *notrans = "N";
  Rcomplex one, zero, *tmp = NULL;
  one.r = 1;
  one.i = 0;
  zero.i =0;
  zero.r = 0;

   tmp = (Rcomplex *) R_Calloc(xrows * ycols, Rcomplex);
  F77_CALL(zgemm)(notrans, notrans, &xrows, &ycols, &xcols, &one, x, &xrows, y, &yrows, &zero, tmp, &xrows FCONE FCONE);
  Memcpy(z, tmp, xrows * ycols);
  R_Free(tmp);
}


void for_c(int *d_v, double *a_v, double *nu1_v, double *C_v, double *nu2_v, 
           int *P, int *N, int *L, int *model, double *u,
           double *a0, double *nu0, double *A, double *B,
           int *sequen, int *largo_sequen, int *n,
           double *coord, double *phi, int *vtype, int *m1, double *simu1, double *L1, double *params_other) {
  
  // Calculate loop iterations
  const int prod = *P * *L * *N;
  const int d = *d_v;
  const int p = *P;
  
  // Allocate memory for temporary variables
  double *f = (double *) R_Calloc(p * p, double);
  double *u_1 = (double *) R_Calloc(d, double);
  double *w = (double *) R_Calloc(p, double);
  
  // Complex numbers for eigenvalues and eigenvectors
  Rcomplex *autovalores = (Rcomplex *) R_Calloc(p, Rcomplex);
  Rcomplex *autovectores = (Rcomplex *) R_Calloc(p * p, Rcomplex);
  double *trabajo_adicional = (double *) R_Calloc(3 * p - 1, double);
  
  // LAPACK parameters
  const int lwork = 3 * p - 1;
  int info;
  const char *J = "V";  // Compute eigenvectors
  const char *U = "L";  // Use lower triangular part
  const size_t N1 = p;
  
  // Allocate memory for complex intermediate matrix
  Rcomplex *vdm = (Rcomplex *) R_Calloc(p * p, Rcomplex);
  Rcomplex raizv[2];
  
  // First phase: Process all spatial locations
  for (int l = 0; l < prod; l++) {
    // Extract the current vector u_1
    for (int i = 0; i < d; i++) {
      u_1[i] = u[l + i * prod];
    }
    
    // Calculate vector norm
    const int g1 = 1;
    const double norm_u = F77_CALL(dnrm2)(&d, u_1, &g1);
    
    // Calculate spectral density matrix based on model
    spectraldensityC(norm_u, *model, d, p, f, a_v, C_v, nu1_v, nu2_v, params_other);
    
    /* Importance-sampling normalization.  Frequencies are generated
       from the Matérn proposal with parameters (a0, nu0), so the
       spectral matrix must be divided elementwise by that proposal
       density.  The previous code used an uninitialised zero-valued
       placeholder and consequently replaced every matrix entry by 2. */
    const double proposal_density = den_mat(norm_u, *nu0, *a0);
    if (!R_FINITE(proposal_density) || proposal_density <= 0.0) {
      for (int i = 0; i < p * p; i++) f[i] = 0.0;
    } else {
      for (int i = 0; i < p * p; i++) {
        f[i] = 2.0 * f[i] / proposal_density;
      }
    }
    
    // Handle matrices of different dimensions
    if (p >= 2) {
      // Calculate eigenvalues and eigenvectors using LAPACK
      F77_CALL(dsyev)(J, U, &p, f, &p, w, trabajo_adicional, &lwork, &info, N1, N1);
      
      // Store eigenvalues in reverse order (largest first)
      for (int i = 0; i < p; i++) {
        autovalores[i].r = w[p - i - 1];
        autovalores[i].i = 0;
      }
      
      // Store eigenvectors (assuming p=2 case)
      autovectores[0].r = f[2];
      autovectores[1].r = f[3];
      autovectores[2].r = f[0];
      autovectores[3].r = f[1];
      
      for (int i = 0; i < p * p; i++) {
        autovectores[i].i = 0;
      }
    } else {
      // Handle the case p=1
      autovalores[0].r = *f;
      autovalores[0].i = 0;
      autovectores[0].r = 1;
      autovectores[0].i = 0;
    }
    
    // Ensure eigenvalues are non-negative
    for (int i = 0; i < p; i++) {
      if (autovalores[i].r < 0) {
        autovalores[i].r = 0;
      }
    }
    
    // Calculate square roots for A and B matrices
    if (p == 1) {
      // Single component case
      Rcomplex raiz;
      if (autovalores[0].r >= 0) {
        raiz.r = sqrt(autovalores[0].r);
        raiz.i = 0;
      } else {
        raiz.i = sqrt(-1 * autovalores[0].r);
        raiz.r = 0;
      }
      
      A[l] = raiz.r;
      B[l] = raiz.i;
    } else {
      // Multiple components case
      for (size_t i = 0; i < p; i++) {
        if (autovalores[i].r >= 0) {
          raizv[i].r = sqrt(autovalores[i].r);
          raizv[i].i = 0;
        } else {
          raizv[i].i = sqrt(-1 * autovalores[i].r);
          raizv[i].r = 0;
        }
      }
      
      // Construct the matrix vdm
      vdm[0].r = raizv[0].r;
      vdm[0].i = raizv[0].i;
      vdm[1].r = 0;
      vdm[2].r = 0;
      vdm[1].i = 0;
      vdm[2].i = 0;
      vdm[3].r = raizv[1].r;
      vdm[3].i = raizv[1].i;
      
      // Calculate matrix products
      C_tcrossprod(vdm, vdm, 2, 2, autovectores, 2, 2);
      C_mult_mat(vdm, autovectores, 2, 2, vdm, 2, 2);
      
      // Calculate indices and update A and B matrices
      const double fo = (double)l / p;
      const int j = l - p * (int)floor(fo);  // Column index
      
      for (int h = 0; h < p; h++) {
        double V, imag_;
        if (j == 0) {
          V = vdm[h].r;
          imag_ = -vdm[h].i;
        } else {
          V = vdm[2 + h].r;
          imag_ = -vdm[2 + h].i;
        }
        A[h + p * l] = V;
        B[h + p * l] = imag_;
      }
    }
  }
  
  // Free first phase memory allocations
  R_Free(f);
  R_Free(u_1);
  R_Free(autovalores);
  R_Free(autovectores);
  R_Free(w);
  R_Free(trabajo_adicional);
  R_Free(vdm);

  // Second phase: Process sequences for simulation
  const int lim = *largo_sequen - 1;
  const double TWO_PI = 2 * M_PI;
  
  for (int i = 0; i < lim; i++) {
    // Calculate dimensions for current sequence
    const int fila = sequen[i + 1] - sequen[i];
    const int col = *n;
    const int sequen1 = sequen[i];
    const int m = fila;
    
    // Allocate memory for coordinates subset
    double *sub_coord = (double *)R_Calloc(fila * d, double);
    extraer(coord, sequen1, sub_coord, fila, col, d);
    
    // Allocate memory for simulation results
    double *simu = (double *)R_Calloc(m * *P * *N, double);
    
    // Process each simulation
    for (size_t k = 0; k < *N; k++) {
      // Calculate index range for current simulation
      const int start_idx = k * *L * *P;
      const int end_idx = (k + 1) * *L * *P;
      const int largo = end_idx - start_idx;
      
      // Allocate and fill index array
      int *index = (int *)R_Calloc(largo, int);
      rellenar_indice(index, start_idx, end_idx, largo);
      
      // Extract u values for current indices
      double *ui = (double *)R_Calloc(largo * d, double);
      u_index_extraer(ui, u, index, largo, d, prod);
      
      // Calculate coordinate products
      double *x0 = (double *)R_Calloc(fila * largo, double);
      tcrossprod(x0, sub_coord, fila, d, ui, largo, d);
      
      // Scale by 2π
      mult_x_cons(x0, TWO_PI, fila * largo);
      
      // Extract phi values
      double *phi_cross = (double *)R_Calloc(largo, double);
      u_index_extraer(phi_cross, phi, index, largo, 1, prod);
      
      // Create constant vector of ones
      double *cbind = (double *)R_Calloc(m, double);
      for (int h = 0; h < m; h++) {
        cbind[h] = 1;
      }
      
      // Calculate phase matrix
      double *x1 = (double *)R_Calloc(fila * largo, double);
      tcrossprod(x1, cbind, m, 1, phi_cross, largo, 1);
      
      // Combine matrices
      double *x = (double *)R_Calloc(fila * largo, double);
      sumar_matrices(x, x1, x0, fila * largo);
      
      // Processing based on variant type
      if (*vtype == 0) {
        if (*P == 1) {
          // Single component variant
          double *x_cos = (double *)R_Calloc(fila * largo, double);
          double *x_sen = (double *)R_Calloc(fila * largo, double);
          cos_vec(x_cos, x, fila * largo);
          sen_vec(x_sen, x, fila * largo);
          
          // Extract A and B components
          double *A_index = (double *)R_Calloc(largo, double);
          double *B_index = (double *)R_Calloc(largo, double);
          u_index_extraer(A_index, A, index, largo, 1, prod);
          u_index_extraer(B_index, B, index, largo, 1, prod);
          
          // Calculate results
          double *result1 = (double *)R_Calloc(m, double);
          double *result2 = (double *)R_Calloc(m, double);
          mult_mat(result1, x_cos, m, largo, A_index, largo, 1);
          mult_mat(result2, x_sen, m, largo, B_index, largo, 1);
          
          // Combine and store results
          sumar_matrices(result2, result1, result2, m);
          llenar_simu(result2, simu, k, P, m);
          
          // Free memory
          R_Free(x_cos);
          R_Free(x_sen);
          R_Free(result1);
          R_Free(result2);
          R_Free(A_index);
          R_Free(B_index);
        } else {
          // Multiple component variant
          double *x_cos = (double *)R_Calloc(fila * largo, double);
          double *x_sen = (double *)R_Calloc(fila * largo, double);
          cos_vec(x_cos, x, fila * largo);
          sen_vec(x_sen, x, fila * largo);
          
          // Extract the selected P x largo blocks in column-major order.
          double *A_col = (double *)R_Calloc(p * largo, double);
          double *B_col = (double *)R_Calloc(p * largo, double);
          for (int q = 0; q < largo; q++) {
            const int source_col = index[q];
            for (int h = 0; h < p; h++) {
              A_col[h + p * q] = A[h + p * source_col];
              B_col[h + p * q] = B[h + p * source_col];
            }
          }
          
          // Calculate cross products
          double *result1 = (double *)R_Calloc(m * *P, double);
          double *result2 = (double *)R_Calloc(m * *P, double);
          tcrossprod(result1, x_cos, m, largo, A_col, p, largo);
          tcrossprod(result2, x_sen, m, largo, B_col, p, largo);
          
          // Combine and store results
          sumar_matrices(result2, result1, result2, m * p);
          llenar_simu(result2, simu, k, P, m);
          
          // Free memory
          R_Free(result1);
          R_Free(result2);
          R_Free(A_col);
          R_Free(B_col);
          R_Free(x_cos);
          R_Free(x_sen);
        }
      } else {
        // Alternative variant processing
        double *cosphi = (double *)R_Calloc(fila * largo, double);
        double *senphi = (double *)R_Calloc(fila * largo, double);
        cos_vec(cosphi, x1, fila * largo);
        sen_vec(senphi, x1, fila * largo);
        
        if (*P == 1) {
          // Unimplemented single component case
          Rprintf("Single component processing for vtype != 0 not implemented\n");
        } else {
          // Multiple component variant
          double *x_cos = (double *)R_Calloc(fila * largo, double);
          double *x_sen = (double *)R_Calloc(fila * largo, double);
          cos_vec(x_cos, x, fila * largo);
          sen_vec(x_sen, x, fila * largo);
          
          // Extract the selected P x largo blocks in column-major order.
          double *A_col = (double *)R_Calloc(p * largo, double);
          double *B_col = (double *)R_Calloc(p * largo, double);
          for (int q = 0; q < largo; q++) {
            const int source_col = index[q];
            for (int h = 0; h < p; h++) {
              A_col[h + p * q] = A[h + p * source_col];
              B_col[h + p * q] = B[h + p * source_col];
            }
          }
          
          // Apply corrections
          restar_matrices(x_cos, x_cos, cosphi, fila * largo);
          restar_matrices(x_sen, x_sen, senphi, fila * largo);
          
          // Calculate cross products
          double *result1 = (double *)R_Calloc(m * *P, double);
          double *result2 = (double *)R_Calloc(m * *P, double);
          tcrossprod(result1, x_cos, m, largo, A_col, p, largo);
          tcrossprod(result2, x_sen, m, largo, B_col, p, largo);
          
          // Combine and store results
          sumar_matrices(result2, result1, result2, m * p);
          llenar_simu(result2, simu, k, P, m);
          
          // Free memory
          R_Free(result1);
          R_Free(result2);
          R_Free(A_col);
          R_Free(B_col);
          R_Free(x_sen);
          R_Free(x_cos);
        }
        
        // Free memory
        R_Free(cosphi);
        R_Free(senphi);
      }
      
      // Free memory for current simulation
      R_Free(phi_cross);
      R_Free(cbind);
      R_Free(x);
      R_Free(x1);
      R_Free(x0);
      R_Free(ui);
      R_Free(index);
    }
    
    // Fill final simulation results
    llenar_simu1(simu1, simu, m1, P, N, lim, i, L1);
    
    // Free memory for current sequence
    R_Free(simu);
    R_Free(sub_coord);
  }
}
