# myrfit2.R
# Optimized standalone version: direct rotation-axis extraction with singular-case fallback.
# Base-R translation of rps_port_ultra_2d3d_fastjj.py, exhaustive rotation mode.
# No Python or reticulate. Requires Rcpp; embedded C++ is compiled on first call.
#
# Numerical equivalence with myrfit() must be tested on representative inputs.

# Embedded C++ acceleration. No separate .cpp file is needed in the package.
# OpenMP is used for the exhaustive landmark-pair loop when supported by Rtools.
# The C++ code is compiled lazily, on the first call to myrfit2().
# Compiled functions live in a private environment and are reused in this R session.
# This avoids package-level src/ and RcppExports registration requirements.
if (!requireNamespace("Rcpp", quietly = TRUE)) {
  stop("Package 'Rcpp' is required by myrfit2().", call. = FALSE)
}

.myrfit2_cpp_env <- new.env(parent = environment())

.myrfit2_ensure_cpp <- function() {
  if (isTRUE(.myrfit2_cpp_env$ready) &&
      exists("rps_rot3d_exhaustive_cpp", envir = .myrfit2_cpp_env, inherits = FALSE) &&
      exists("rps_fast_kernels", envir = .myrfit2_cpp_env, inherits = FALSE)) {
    return(invisible(TRUE))
  }
  if (!requireNamespace("Rcpp", quietly = TRUE)) {
    stop("Package 'Rcpp' is required by myrfit2().", call. = FALSE)
  }
  # Compile each kernel only once per R session. Rcpp's cache is retained in tempdir().
  Rcpp::cppFunction(code = '

#include <Rcpp.h>
#include <algorithm>
#include <cmath>
#include <vector>
using namespace Rcpp;

static inline double norm3(double x, double y, double z) {
  return std::sqrt(x*x + y*y + z*z);
}
static inline void cross3(const double* a, const double* b, double* c) {
  c[0] = a[1]*b[2]-a[2]*b[1];
  c[1] = a[2]*b[0]-a[0]*b[2];
  c[2] = a[0]*b[1]-a[1]*b[0];
}
// Median of x using reusable scratch storage. The input is preserved because
// the axis-sign correction loop relies on landmark order.
static inline double medv_scratch(const std::vector<double>& x, std::vector<double>& scratch) {
  const size_t n=x.size();
  if (!n) return NA_REAL;
  scratch.assign(x.begin(),x.end());
  const size_t m=n/2;
  std::nth_element(scratch.begin(),scratch.begin()+m,scratch.end());
  double hi=scratch[m];
  if (n%2) return hi;
  std::nth_element(scratch.begin(),scratch.begin()+m-1,scratch.begin()+m);
  return 0.5*(scratch[m-1]+hi);
}
// Null direction of H=R-I from the smallest eigenvector of H\'H,
// via a 3x3 symmetric Jacobi eigensolver (avoids thousands of R eigen() calls).
static void axis_from_R(const double R[9], double angle, double* axis) {
  // Fast closed-form axis for ordinary rotations. The antisymmetric part of R
  // gives 2*sin(angle)*axis; canonicalize the sign to match the SVD convention.
  // Near 0 or pi this formula is ill-conditioned, so retain the Jacobi fallback.
  double sa = std::sin(angle);
  if (std::fabs(sa) > 1e-7) {
    axis[0] = R[7] - R[5];
    axis[1] = R[2] - R[6];
    axis[2] = R[3] - R[1];
    double nn = norm3(axis[0], axis[1], axis[2]);
    if (nn > 0.0 && std::isfinite(nn)) {
      for (int i=0; i<3; ++i) axis[i] /= nn;
      if (axis[2] < 0.0) for (int i=0; i<3; ++i) axis[i] = -axis[i];
      for (int i=0; i<3; ++i) if (std::fabs(axis[i]) < 1e-11) axis[i] = 0.0;
      return;
    }
  }
  double H[9], M[9]={0,0,0,0,0,0,0,0,0};
  int count=1;
  for(int r=0;r<3;++r) for(int c=0;c<3;++c) {
    H[3*r+c]=R[3*r+c]-(r==c?1.0:0.0);
    if(std::fabs(H[3*r+c])<1e-11) ++count;
  }
  if(count>=9) { axis[0]=0; axis[1]=0; axis[2]=1; return; }
  for(int r=0;r<3;++r) for(int c=0;c<3;++c)
    for(int k=0;k<3;++k) M[3*r+c]+=H[3*k+r]*H[3*k+c];
  double V[9]={1,0,0,0,1,0,0,0,1};
  for(int it=0;it<30;++it) {
    int p=0,q=1; double mx=std::fabs(M[1]);
    if(std::fabs(M[2])>mx){p=0;q=2;mx=std::fabs(M[2]);}
    if(std::fabs(M[5])>mx){p=1;q=2;mx=std::fabs(M[5]);}
    if(mx<1e-15) break;
    double app=M[3*p+p], aqq=M[3*q+q], apq=M[3*p+q];
    double tau=(aqq-app)/(2.0*apq);
    double t=(tau>=0?1.0:-1.0)/(std::fabs(tau)+std::sqrt(1.0+tau*tau));
    double cs=1.0/std::sqrt(1.0+t*t), sn=t*cs;
    for(int k=0;k<3;++k) if(k!=p && k!=q) {
      double mkp=M[3*k+p], mkq=M[3*k+q];
      M[3*k+p]=M[3*p+k]=cs*mkp-sn*mkq;
      M[3*k+q]=M[3*q+k]=sn*mkp+cs*mkq;
    }
    M[3*p+p]=app-t*apq; M[3*q+q]=aqq+t*apq; M[3*p+q]=M[3*q+p]=0.0;
    for(int k=0;k<3;++k) {
      double vkp=V[3*k+p], vkq=V[3*k+q];
      V[3*k+p]=cs*vkp-sn*vkq; V[3*k+q]=sn*vkp+cs*vkq;
    }
  }
  int idx=0; if(M[4]<M[0])idx=1; if(M[8]<M[3*idx+idx])idx=2;
  axis[0]=V[idx]; axis[1]=V[3+idx]; axis[2]=V[6+idx];
  for(int i=0;i<3;++i) if(std::fabs(axis[i])<1e-11) axis[i]=0.0;
  if(axis[2]<0) for(int i=0;i<3;++i) axis[i]=-axis[i];
  double nn=norm3(axis[0],axis[1],axis[2]);
  if(!(nn>0) || !std::isfinite(nn)){axis[0]=0;axis[1]=0;axis[2]=1;}
  else for(int i=0;i<3;++i) axis[i]/=nn;
}
// [[Rcpp::export]]
NumericVector rps_rot3d_exhaustive_cpp(NumericMatrix X, NumericMatrix Y) {
  int p=X.nrow();
  if(X.ncol()!=3 || Y.nrow()!=p || Y.ncol()!=3) stop("X and Y must be p x 3 matrices");
  std::vector<double> axes((size_t)p*p*3,0.0), angles((size_t)p*p,0.0);
  auto axidx=[&](int i,int j,int c)->size_t{return ((size_t)i*p+j)*3+c;};
  auto anidx=[&](int i,int j)->size_t{return (size_t)i*p+j;};
  // Each unordered landmark pair writes to its own slots; pair calculations are independent.
  // OpenMP is enabled by the embedded Rcpp plugin where supported.
  #pragma omp parallel for schedule(static) if(p >= 64)
  for(int i=1;i<p;++i) {
    for(int j=0;j<i;++j) {
    double dx[3],dy[3];
    for(int c=0;c<3;++c){dx[c]=X(j,c)-X(i,c);dy[c]=Y(j,c)-Y(i,c);}
    // Python estimorot3D uses X[j]-X[i] and Y[j]-Y[i].
    double nx=norm3(dx[0],dx[1],dx[2]), ny=norm3(dy[0],dy[1],dy[2]);
    double R[9]={1,0,0,0,1,0,0,0,1};
    if(nx>0 && ny>0) {
      double a0[3]={dx[0]/nx,dx[1]/nx,dx[2]/nx};
      double b0[3]={dy[0]/ny,dy[1]/ny,dy[2]/ny};
      double v[3],w[3];
      double xj[3]={X(j,0),X(j,1),X(j,2)}, yj[3]={Y(j,0),Y(j,1),Y(j,2)};
      cross3(a0,xj,v); cross3(b0,yj,w);
      double nv=norm3(v[0],v[1],v[2]), nw=norm3(w[0],w[1],w[2]);
      double A[9],B[9]; bool good=false;
      if(nv>1e-11 && nw>1e-11) {
        double a1[3]={v[0]/nv,v[1]/nv,v[2]/nv}, b1[3]={w[0]/nw,w[1]/nw,w[2]/nw};
        double a2[3],b2[3]; cross3(a0,a1,a2); cross3(b0,b1,b2);
        double na2=norm3(a2[0],a2[1],a2[2]), nb2=norm3(b2[0],b2[1],b2[2]);
        if(na2>0 && nb2>0) {
          for(int c=0;c<3;++c){A[c]=a0[c];A[3+c]=a1[c];A[6+c]=a2[c]/na2;B[c]=b0[c];B[3+c]=b1[c];B[6+c]=b2[c]/nb2;} good=true;
        }
      } else if(nv<1e-11 && nw<1e-11) {
        double u[3]; cross3(a0,b0,u); double nu=norm3(u[0],u[1],u[2]);
        if(nu>=1e-11) {
          for(int c=0;c<3;++c)u[c]/=nu;
          double a2[3],b2[3]; cross3(a0,u,a2); cross3(b0,u,b2);
          double na2=norm3(a2[0],a2[1],a2[2]), nb2=norm3(b2[0],b2[1],b2[2]);
          if(na2>0 && nb2>0) {
            for(int c=0;c<3;++c){A[c]=a0[c];A[3+c]=u[c];A[6+c]=a2[c]/na2;B[c]=b0[c];B[3+c]=u[c];B[6+c]=b2[c]/nb2;} good=true;
          }
        }
      }
      if(good) for(int r=0;r<3;++r) for(int c=0;c<3;++c) {
        R[3*r+c]=0.0; for(int k=0;k<3;++k) R[3*r+c]+=A[3*k+r]*B[3*k+c];
      }
    }
    double tr=R[0]+R[4]+R[8];
    double ang=std::acos(std::max(-1.0,std::min(1.0,(tr-1.0)/2.0)));
    double axis[3]; axis_from_R(R,ang,axis);
    for(int c=0;c<3;++c){axes[axidx(i,j,c)]=axis[c];axes[axidx(j,i,c)]=axis[c];}
    angles[anidx(j,i)]=ang; angles[anidx(i,j)]=-ang;
    }
  }
  std::vector<double> ejes((size_t)p*3,0.0);
  std::vector<double> scratch; scratch.reserve((size_t)p);
  std::vector<double> cols[3];
  for(int c=0;c<3;++c) cols[c].reserve((size_t)p);
  for(int k=0;k<p;++k) {
    for(int c=0;c<3;++c) cols[c].clear();
    for(int h=0;h<p;++h) for(int c=0;c<3;++c)
      cols[c].push_back(h==k ? (c==2?1.0:0.0) : axes[axidx(h,k,c)]);
    double v[3]={medv_scratch(cols[0],scratch),medv_scratch(cols[1],scratch),medv_scratch(cols[2],scratch)};
    double nv=norm3(v[0],v[1],v[2]); int count=0;
    while(nv<0.001 && count<p) {
      int h=count;
      if(h!=k) {
        double z=axes[axidx(h,k,2)], xx=axes[axidx(h,k,0)];
        if(z!=1.0 && xx<0) for(int c=0;c<3;++c) cols[c][h]=-cols[c][h];
      }
      v[0]=medv_scratch(cols[0],scratch);v[1]=medv_scratch(cols[1],scratch);v[2]=medv_scratch(cols[2],scratch);
      nv=norm3(v[0],v[1],v[2]); ++count;
    }
    if(nv>0) for(int c=0;c<3;++c)ejes[(size_t)k*3+c]=v[c]/nv;
    else ejes[(size_t)k*3+2]=1.0;
  }
  std::vector<double> medaxes[3];
  for(int c=0;c<3;++c) medaxes[c].reserve((size_t)p);
  for(int k=0;k<p;++k) for(int c=0;c<3;++c) medaxes[c].push_back(ejes[(size_t)k*3+c]);
  double eje[3]={medv_scratch(medaxes[0],scratch),medv_scratch(medaxes[1],scratch),medv_scratch(medaxes[2],scratch)};
  double ne=norm3(eje[0],eje[1],eje[2]); if(ne>0)for(int c=0;c<3;++c)eje[c]/=ne; else {eje[0]=0;eje[1]=0;eje[2]=1;}
  std::vector<double> rowmed; rowmed.reserve((size_t)p);
  std::vector<double> vals; vals.reserve((size_t)(p-1));
  for(int i=0;i<p;++i){vals.clear();for(int j=0;j<p;++j)if(i!=j)vals.push_back(angles[anidx(i,j)]);rowmed.push_back(medv_scratch(vals,scratch));}
  double ang=medv_scratch(rowmed,scratch);
  return NumericVector::create(_["axis1"]=eje[0],_["axis2"]=eje[1],_["axis3"]=eje[2],_["angle"]=ang);
}
',
    env = .myrfit2_cpp_env, depends = "Rcpp", plugins = c("cpp11", "openmp"), rebuild = FALSE)

  Rcpp::cppFunction(code = '

#include <Rcpp.h>
#include <algorithm>
#include <cmath>
#include <vector>
using namespace Rcpp;
static double med_cpp(std::vector<double>& v) {
  if(v.empty()) return NA_REAL;
  size_t n=v.size(), m=n/2;
  std::nth_element(v.begin(),v.begin()+m,v.end());
  double hi=v[m];
  if(n%2) return hi;
  std::nth_element(v.begin(),v.begin()+m-1,v.begin()+m);
  return 0.5*(v[m-1]+hi);
}
static double normrow_cpp(const NumericMatrix& A, int i, const NumericVector& center) {
  double s=0; for(int c=0;c<A.ncol();++c){double z=A(i,c)-center[c];s+=z*z;} return std::sqrt(s);
}
static NumericVector spatial_med_cpp(NumericMatrix A, double tol, int max_iter) {
  int n=A.nrow(), d=A.ncol(); NumericVector aux(d), prev(d), w(n,1.0);
  for(int c=0;c<d;++c){for(int i=0;i<n;++i)aux[c]+=A(i,c);aux[c]/=n;}
  bool sensor=false; int h=1;
  double delta=R_PosInf;
  while(delta>tol && h<=max_iter) {
    std::vector<double> dist(n); bool hit_any=false;
    for(int i=0;i<n;++i){dist[i]=normrow_cpp(A,i,aux);if(dist[i]==0)hit_any=true;}
    if(hit_any)sensor=true;
    std::vector<double> sw(n); double denom=0;
    for(int i=0;i<n;++i){sw[i]=(dist[i]==0?tol:w[i]/dist[i]);denom+=sw[i];}
    if(denom==0)break;
    NumericVector weighted(d);
    for(int i=0;i<n;++i)for(int c=0;c<d;++c)weighted[c]+=A(i,c)*sw[i];
    NumericVector tdemu(d); double wn=0;
    for(int c=0;c<d;++c){tdemu[c]=weighted[c]/denom;wn+=weighted[c]*weighted[c];}
    double gamma=std::min(1.0,std::min(sensor?1.0:0.0,std::sqrt(wn)));
    delta=0;
    for(int c=0;c<d;++c){prev[c]=aux[c];aux[c]=(1-gamma)*tdemu[c]+gamma*aux[c];double z=aux[c]-prev[c];delta+=z*z;}
    delta=std::sqrt(delta); ++h;
  }
  return aux;
}
// op: spatial (matrix n x d), focus/config (array p x d x n),
// scale_specimen (matrix p x d in x, p x d in y).
// [[Rcpp::export]]
NumericVector rps_fast_kernels(NumericVector x, std::string op,
                               Nullable<NumericVector> y=R_NilValue,
                               double tol=1e-9, int max_iter=1000) {
  if(op=="spatial") {
    IntegerVector dm=x.attr("dim"); if(dm.size()!=2) stop("spatial expects a matrix");
    NumericMatrix A(x); return spatial_med_cpp(A,tol,max_iter);
  }
  if(op=="focus" || op=="config") {
    IntegerVector dm=x.attr("dim"); if(dm.size()!=3) stop("focus/config expects p x d x n array");
    int p=dm[0],d=dm[1],n=dm[2]; NumericVector out=clone(x);
    if(op=="focus") {
      for(int k=0;k<n;++k){NumericMatrix A(p,d);for(int c=0;c<d;++c)for(int i=0;i<p;++i)A(i,c)=out[i+p*c+p*d*k];NumericVector cen=spatial_med_cpp(A,tol,max_iter);for(int c=0;c<d;++c)for(int i=0;i<p;++i)out[i+p*c+p*d*k]-=cen[c];}
      return out;
    }
    out=NumericVector(p*d); out.attr("dim")=IntegerVector::create(p,d);
    for(int i=0;i<p;++i){NumericMatrix A(n,d);for(int k=0;k<n;++k)for(int c=0;c<d;++c)A(k,c)=x[i+p*c+p*d*k];NumericVector cen=spatial_med_cpp(A,tol,max_iter);for(int c=0;c<d;++c)out[i+p*c]=cen[c];}
    return out;
  }
  if(op=="scale_specimen") {
    IntegerVector dm=x.attr("dim"); if(dm.size()!=2 || y.isNull())stop("scale_specimen expects two matrices");
    NumericMatrix X(x),Y(as<NumericVector>(y)); int p=X.nrow(),d=X.ncol();
    std::vector<double> rowmed(p);
    for(int i=0;i<p;++i){std::vector<double> ratios;ratios.reserve(p-1);for(int j=0;j<p;++j)if(i!=j){double dx=0,dy=0;for(int c=0;c<d;++c){double a=X(i,c)-X(j,c),b=Y(i,c)-Y(j,c);dx+=a*a;dy+=b*b;}dx=std::sqrt(dx);dy=std::sqrt(dy);ratios.push_back(dx==0?(dy>0?R_PosInf:NA_REAL):dy/dx);}std::vector<double> clean;for(double v:ratios)if(!std::isnan(v))clean.push_back(v);rowmed[i]=med_cpp(clean);}
    std::vector<double> clean;for(double v:rowmed)if(!std::isnan(v))clean.push_back(v);double rho=med_cpp(clean);NumericVector out(p*d);out.attr("dim")=IntegerVector::create(p,d);for(int c=0;c<d;++c)for(int i=0;i<p;++i)out[i+p*c]=rho*X(i,c);return out;
  }
  stop("Unknown rps_fast_kernels operation");
  return NumericVector(0);
}
',
    env = .myrfit2_cpp_env, depends = "Rcpp", plugins = "cpp11", rebuild = FALSE)
  .myrfit2_cpp_env$ready <- TRUE
  invisible(TRUE)
}

rps_rot3d_exhaustive_cpp <- function(X, Y) {
  .myrfit2_ensure_cpp()
  .myrfit2_cpp_env$rps_rot3d_exhaustive_cpp(X, Y)
}

rps_fast_kernels <- function(x, op, y = NULL, tol = 1e-9, max_iter = 1000L) {
  .myrfit2_ensure_cpp()
  .myrfit2_cpp_env$rps_fast_kernels(x, op, y, tol, max_iter)
}

.rps_norm <- function(x) sqrt(sum(x * x))

.rps_spatial_median <- function(A, tol = 1e-9, max_iter = 1000L) {
  rps_fast_kernels(as.matrix(A), "spatial", tol=tol, max_iter=as.integer(max_iter))
}

.rps_initial_focus <- function(X) {
  rps_fast_kernels(X, "focus", tol=1e-9, max_iter=1000L)
}

.rps_spatialmed_config <- function(X) {
  rps_fast_kernels(X, "config", tol=1e-9, max_iter=1000L)
}

.rps_medland <- function(X) {
  p <- nrow(X)
  ij <- utils::combn(p, 2L)
  delta <- X[ij[1L,],,drop=FALSE] - X[ij[2L,],,drop=FALSE]
  median(sqrt(rowSums(delta*delta)))
}

.rps_adjustment_scale <- function(X) {
  n <- dim(X)[3L]
  for (k in seq_len(n)) {
    s <- .rps_medland(X[,,k,drop=FALSE][,,1L])
    if (!is.finite(s) || s <= 0) stop("Degenerate configuration at specimen ", k)
    X[,,k] <- X[,,k,drop=FALSE][,,1L] / s
  }
  X
}

.rps_scale_specimen <- function(X, Y) {
  rps_fast_kernels(as.matrix(X), "scale_specimen", as.matrix(Y))
}

.rps_cross <- function(a,b) c(a[2]*b[3]-a[3]*b[2],
                              a[3]*b[1]-a[1]*b[3],
                              a[1]*b[2]-a[2]*b[1])

.rps_estimorot3d <- function(X,Y,i,j,tol_norm=1e-11) {
  dx <- X[j,]-X[i,]; dy <- Y[j,]-Y[i,]
  nx <- .rps_norm(dx); ny <- .rps_norm(dy)
  if (nx == 0 || ny == 0) return(diag(3L))
  a0 <- dx/nx; b0 <- dy/ny
  v <- .rps_cross(a0,X[j,]); w <- .rps_cross(b0,Y[j,])
  nv <- .rps_norm(v); nw <- .rps_norm(w)
  if (nv > tol_norm && nw > tol_norm) {
    a1 <- v/nv; b1 <- w/nw
    a2 <- .rps_cross(a0,a1); b2 <- .rps_cross(b0,b1)
    na2 <- .rps_norm(a2); nb2 <- .rps_norm(b2)
    if (na2 > 0 && nb2 > 0) {
      A <- rbind(a0,a1,a2/na2)
      B <- rbind(b0,b1,b2/nb2)
      return(t(A)%*%B)
    }
  } else if (nv < tol_norm && nw < tol_norm) {
    u <- .rps_cross(a0,b0); nu <- .rps_norm(u)
    if (nu >= tol_norm) {
      u <- u/nu
      a2 <- .rps_cross(a0,u); b2 <- .rps_cross(b0,u)
      na2 <- .rps_norm(a2); nb2 <- .rps_norm(b2)
      if (na2 > 0 && nb2 > 0) {
        A <- rbind(a0,u,a2/na2)
        B <- rbind(b0,u,b2/nb2)
        return(t(A)%*%B)
      }
    }
  }
  diag(3L)
}

.rps_axis_from_rotation <- function(R, tol=1e-11) {
  for (i in 1:3) if (abs(R[i,1]) < 1e-16) R[i,1] <- 0
  H <- R-diag(3L)
  dif <- abs(H)
  count <- 1L + sum(dif < tol)
  if (count < 9L) {
    M <- t(H)%*%H
    ee <- eigen(M, symmetric=TRUE)
    # MASS::Null convention: right singular vectors whose singular values
    # are <= sqrt(eps) times the largest singular value of M.
    # The singular values of the symmetric positive-semidefinite M are
    # its eigenvalues (not their square roots).
    sv <- pmax(ee$values, 0)
    dmax <- max(sv)
    keep <- if (dmax > 0) which(sv <= sqrt(.Machine$double.eps)*dmax) else seq_along(sv)
    # Numerical round-off can leave no eigenvalue under the relative cutoff.
    # The null direction is then approximated by the smallest-eigenvalue vector.
    idx <- if (length(keep)) keep[1L] else which.min(ee$values)
    V <- ee$vectors[,idx]
  } else V <- c(0,0,1)
  if (any(!is.finite(V))) V <- c(0,0,1)
  V[abs(V)<tol] <- 0
  if (V[3] < 0) V <- -V
  nv <- .rps_norm(V)
  if (nv == 0) c(0,0,1) else V/nv
}

.rps_rot3d_exhaustive <- function(X,Y) {
  z <- rps_rot3d_exhaustive_cpp(X,Y)
  list(axis=unname(as.numeric(z[1:3])), angle=unname(as.numeric(z[4])))
}

.rps_matrizrot3d <- function(e,a) {
  e <- as.numeric(e); ne <- .rps_norm(e)
  if (ne==0) e <- c(0,0,1) else e <- e/ne
  # MASS::Null(t(e)) uses an SVD-derived orthonormal complement.
  sv <- svd(matrix(e,nrow=1L),nu=0,nv=3L)
  AF <- rbind(sv$v[,2L],sv$v[,3L],e)
  Bloque <- diag(3L)
  Bloque[1:2,1:2] <- matrix(c(cos(a),-sin(a),sin(a),cos(a)),2L,2L)
  t(AF)%*%Bloque%*%AF
}

.rps_rotation <- function(X,Y) {
  z <- .rps_rot3d_exhaustive(X,Y)
  H <- .rps_matrizrot3d(z$axis,z$angle)
  list(X=X%*%H,H=H)
}

.rps_translation <- function(X,Y) {
  X + matrix(apply(Y-X,2L,median),nrow(X),ncol(X),byrow=TRUE)
}

myrfit2 <- function(shapes, consenso=FALSE, tol=1e-9, iter_total=20L,
                    verbose=FALSE, parallel=FALSE, ncores=NULL) {
  .myrfit2_ensure_cpp()
  X <- as.array(shapes)
  if (length(dim(X)) != 3L) stop("shapes must be an array p x d x n")
  storage.mode(X) <- "double"
  p <- dim(X)[1L]; d <- dim(X)[2L]; n <- dim(X)[3L]
  if (!d %in% c(2L,3L)) stop("Only 2-D and 3-D coordinates are supported")
  if (n < 2L) stop("At least two specimens are required")
  if (any(!is.finite(X))) stop("Input contains non-finite coordinates")
  was2d <- d==2L
  # Python reference promotes 2-D to 3-D before centering/scaling/rotation.
  if (was2d) {
    X3 <- array(0,c(p,3L,n)); X3[,1:2,] <- X; X <- X3
  }
  X <- .rps_adjustment_scale(.rps_initial_focus(X))
  Y <- .rps_spatialmed_config(X)
  res <- matrix(0,p,n); resme <- numeric(n)
  for (k in seq_len(n)) {
    res[,k] <- sqrt(rowSums((Y-X[,,k])^2))
    resme[k] <- sum(res[,k])
  }
  residual <- sum(resme); residualant <- 0
  Aux <- X
  history <- residual
  z <- 1L
  if (verbose) message(sprintf("RPS initial: criterion=%.12g; delta=%.12g",
                               residual,abs(residual-residualant)))
  while (z <= iter_total && abs(residual-residualant) > tol) {
    auxresme <- numeric(n)
    candidates <- array(0,dim(X))
    for (k in seq_len(n)) {
      candidate <- .rps_scale_specimen(Aux[,,k],Y)
      candidate <- .rps_rotation(candidate,Y)$X
      candidate <- .rps_translation(candidate,Y)
      candidates[,,k] <- candidate
      auxresme[k] <- median(sqrt(rowSums((Y-candidate)^2)))
    }
    # Persistent Aux update, including candidates rejected from X.
    Aux <- candidates
    for (k in seq_len(n)) if (auxresme[k] <= resme[k]) X[,,k] <- candidates[,,k]
    for (k in seq_len(n)) {
      res[,k] <- sqrt(rowSums((Y-X[,,k])^2))
      resme[k] <- sum(res[,k])
    }
    Y <- .rps_spatialmed_config(X)
    residualant <- residual; residual <- sum(resme)
    history <- c(history,residual)
    if (verbose) message(sprintf("RPS iteration %d: criterion=%.12g; delta=%.12g",
                                 z,residual,abs(residual-residualant)))
    z <- z+1L
  }
  aligned <- X
  if (was2d) {
    aligned <- X[,1:2,,drop=FALSE]
    Y <- Y[,1:2,drop=FALSE]
  }
  if (consenso) {
    out <- array(0,c(p,dim(aligned)[2L],n+1L))
    out[,,seq_len(n)] <- aligned
    out[,,n+1L] <- Y
  } else out <- aligned
  attr(out,"consensus") <- Y
  attr(out,"iterations") <- z-1L
  attr(out,"history") <- history
  attr(out,"converged") <- abs(residual-residualant)<=tol
  out
}
