/* engram_geometry.c — M9 FOUNDATION: relational-neighborhood geometry descriptor.
 * See engram_geometry.h. Pure C11, stdlib + libm. READ-ONLY over store + vindex.
 */
#include "engram_geometry.h"

#include <stdlib.h>
#include <string.h>
#include <math.h>

/* Must match ENGRAM_HEBB_GAIN in el_runtime.c (eff = weight*(1+GAIN*hebb)). */
#define GEO_HEBB_GAIN 0.5
/* Internal cap on the m×m Jacobi eigensolve: above this we still give centroid +
 * radius but skip principal axes (honest degradation, not a lie). */
#define GEO_EIG_CAP 512

/* ───────────────────────── small dynamic member table ──────────────────────
 * Neighborhoods are small (tens..few hundred), so linear-scan dedup is fine. */
typedef struct {
    char** id;          /* strdup'd ids                                        */
    double* memb;       /* provisional membership                              */
    float** emb;        /* L2-normalized emb copy (dim floats) or NULL         */
    double* sal;        /* stored salience                                     */
    int   n, cap, dim;
} MemSet;

static int ms_init(MemSet* s, int dim){
    s->n=0; s->cap=16; s->dim=dim;
    s->id=calloc(s->cap,sizeof*s->id); s->memb=calloc(s->cap,sizeof*s->memb);
    s->emb=calloc(s->cap,sizeof*s->emb); s->sal=calloc(s->cap,sizeof*s->sal);
    return (s->id&&s->memb&&s->emb&&s->sal)?0:-1;
}
static int ms_find(const MemSet* s, const char* id){
    for(int i=0;i<s->n;i++) if(strcmp(s->id[i],id)==0) return i;
    return -1;
}
/* Insert or bump membership (keep the max). Returns member index or <0 on OOM. */
static int ms_upsert(MemSet* s, const char* id, double memb){
    int i=ms_find(s,id);
    if(i>=0){ if(memb>s->memb[i]) s->memb[i]=memb; return i; }
    if(s->n==s->cap){
        int nc=s->cap*2;
        char** a=realloc(s->id,nc*sizeof*a); if(!a) return -1; s->id=a;
        double* b=realloc(s->memb,nc*sizeof*b); if(!b) return -1; s->memb=b;
        float** c=realloc(s->emb,nc*sizeof*c); if(!c) return -1; s->emb=c;
        double* d=realloc(s->sal,nc*sizeof*d); if(!d) return -1; s->sal=d;
        s->cap=nc;
    }
    s->id[s->n]=strdup(id); if(!s->id[s->n]) return -1;
    s->memb[s->n]=memb; s->emb[s->n]=NULL; s->sal[s->n]=0.0;
    return s->n++;
}
static void ms_free(MemSet* s){
    for(int i=0;i<s->n;i++){ free(s->id[i]); free(s->emb[i]); }
    free(s->id); free(s->memb); free(s->emb); free(s->sal);
}

/* L2-normalize a copy of v into out (dim floats). Returns 0, or -1 if ~zero. */
static int normcopy(const float* v, int dim, float* out){
    double s=0; for(int i=0;i<dim;i++) s+=(double)v[i]*v[i];
    double n=sqrt(s); if(n<1e-12) return -1;
    for(int i=0;i<dim;i++) out[i]=(float)(v[i]/n);
    return 0;
}
static double dotf(const float* a, const float* b, int dim){
    double s=0; for(int i=0;i<dim;i++) s+=(double)a[i]*b[i]; return s;
}

/* Attach a member's emb (normalized) + salience by point-reading the store. */
static void ms_load_node(MemSet* s, int i, EngramPagedStore* st){
    StoreNode nn; memset(&nn,0,sizeof nn);
    if(store_get_node(st, s->id[i], &nn)!=1){ return; }
    s->sal[i]=nn.salience;
    if(nn.emb && nn.emb_dim==s->dim){
        float* e=malloc((size_t)s->dim*sizeof(float));
        if(e && normcopy(nn.emb,s->dim,e)==0) s->emb[i]=e; else free(e);
    }
    store_node_free(&nn);
}

/* ───────────────────────── Jacobi symmetric eigensolver ─────────────────────
 * Cyclic Jacobi on a dense symmetric m×m matrix A (row-major, overwritten).
 * Eigenvalues -> w[m]; eigenvectors (columns) -> V[m*m]. Robust, libm-only. */
static void jacobi_sym(double* A, int m, double* w, double* V){
    for(int i=0;i<m;i++){ for(int j=0;j<m;j++) V[i*m+j]=(i==j)?1.0:0.0; }
    for(int sweep=0; sweep<100; sweep++){
        double off=0; for(int p=0;p<m;p++) for(int q=p+1;q<m;q++) off+=A[p*m+q]*A[p*m+q];
        if(off < 1e-18) break;
        for(int p=0;p<m;p++) for(int q=p+1;q<m;q++){
            double apq=A[p*m+q]; if(fabs(apq)<1e-300) continue;
            double app=A[p*m+p], aqq=A[q*m+q];
            double phi=0.5*atan2(2*apq, aqq-app);
            double c=cos(phi), sn=sin(phi);
            for(int k=0;k<m;k++){
                double akp=A[k*m+p], akq=A[k*m+q];
                A[k*m+p]=c*akp - sn*akq; A[k*m+q]=sn*akp + c*akq;
            }
            for(int k=0;k<m;k++){
                double apk=A[p*m+k], aqk=A[q*m+k];
                A[p*m+k]=c*apk - sn*aqk; A[q*m+k]=sn*apk + c*aqk;
            }
            for(int k=0;k<m;k++){
                double vkp=V[k*m+p], vkq=V[k*m+q];
                V[k*m+p]=c*vkp - sn*vkq; V[k*m+q]=sn*vkp + c*vkq;
            }
        }
    }
    for(int i=0;i<m;i++) w[i]=A[i*m+i];
}

void engram_geo_default_params(GeoParams* p){
    if(!p) return;
    p->ann_k=24; p->hop_relational=1; p->edge_min_weight=0.05;
    p->kcore_k=0; p->top_axes=8; p->max_members=400;
}

/* Effective hebb-weighted edge strength, matching eg_edge_eff_weight. */
static double eff_w(double weight, double hebb){
    double w = weight * (1.0 + GEO_HEBB_GAIN*hebb);
    if(w>1.0) w=1.0; if(w<0.0) w=0.0; return w;
}

GeoDescriptor* engram_geometry_descriptor(
    EngramPagedStore* store, VIndex* vindex,
    char** vids, int n_vids,
    const char* const* seed_ids, size_t n_seeds,
    const GeoParams* params)
{
    if(!store || !seed_ids || n_seeds==0) return NULL;
    GeoParams P; if(params) P=*params; else engram_geo_default_params(&P);

    int dim = 0;
    /* infer dim from the first embedded seed */
    for(size_t i=0;i<n_seeds && dim==0;i++){
        StoreNode nn; memset(&nn,0,sizeof nn);
        if(store_get_node(store, seed_ids[i], &nn)==1){
            if(nn.emb && nn.emb_dim>0) dim=nn.emb_dim;
        }
        store_node_free(&nn);
    }
    if(dim==0) dim = 768; /* no embedded seed: still build the relational side */

    MemSet ms; if(ms_init(&ms,dim)!=0){ ms_free(&ms); return NULL; }

    /* 1. seeds (membership 1.0) */
    for(size_t i=0;i<n_seeds;i++) ms_upsert(&ms, seed_ids[i], 1.0);
    int n_seed_members = ms.n;
    for(int i=0;i<ms.n;i++) ms_load_node(&ms,i,store);

    /* provisional centroid from seed embeddings (for the ANN query) */
    float* prov = calloc((size_t)dim,sizeof(float));
    int prov_n=0;
    for(int i=0;i<n_seed_members;i++) if(ms.emb[i]){
        for(int d=0;d<dim;d++) prov[d]+=ms.emb[i][d]; prov_n++;
    }
    if(prov_n){ for(int d=0;d<dim;d++) prov[d]/=(float)prov_n; }

    /* 2. semantic expansion via vindex ANN around the provisional centroid */
    if(vindex && vids && P.ann_k>0 && prov_n>0){
        int k=P.ann_k*(int)n_seeds; if(k<P.ann_k) k=P.ann_k; if(k>n_vids) k=n_vids;
        uint64_t* rids=malloc((size_t)k*sizeof(uint64_t));
        float* dd=malloc((size_t)k*sizeof(float));
        if(rids&&dd){
            int got=vindex_search(vindex, prov, k, 0, rids, dd);
            for(int r=0;r<got;r++){
                if(rids[r]>=(uint64_t)n_vids) continue;
                double memb = 1.0 - (double)dd[r]; /* cosine sim in [-1,1] */
                if(memb<0) memb=0;
                int mi=ms_upsert(&ms, vids[rids[r]], memb*0.9); /* <1: not a seed */
                if(mi>=0 && !ms.emb[mi]) ms_load_node(&ms,mi,store);
            }
        }
        free(rids); free(dd);
    }
    free(prov);

    /* 3. relational expansion: seeds' hebb neighbors become members */
    if(P.hop_relational){
        for(int i=0;i<n_seed_members;i++){
            StoreEdge* es=NULL; size_t ne=0;
            if(store_get_edges_from(store, ms.id[i], &es, &ne)==0 && es){
                for(size_t e=0;e<ne;e++){
                    if(es[e].tombstoned || es[e].inhibitory) continue;
                    double w=eff_w(es[e].weight, es[e].hebb);
                    if(w < P.edge_min_weight) continue;
                    int mi=ms_upsert(&ms, es[e].to_id, w);
                    if(mi>=0 && !ms.emb[mi]) ms_load_node(&ms,mi,store);
                }
            }
            store_edges_free(es,ne);
            es=NULL; ne=0;
            if(store_get_edges_to(store, ms.id[i], &es, &ne)==0 && es){
                for(size_t e=0;e<ne;e++){
                    if(es[e].tombstoned || es[e].inhibitory) continue;
                    double w=eff_w(es[e].weight, es[e].hebb);
                    if(w < P.edge_min_weight) continue;
                    int mi=ms_upsert(&ms, es[e].from_id, w);
                    if(mi>=0 && !ms.emb[mi]) ms_load_node(&ms,mi,store);
                }
            }
            store_edges_free(es,ne);
        }
    }

    /* optional cap: keep the highest-membership members (guards eigensolve) */
    if(P.max_members>0 && ms.n>P.max_members){
        /* simple selection: repeatedly drop the min-membership non-seed member */
        while(ms.n>P.max_members){
            int worst=-1; double wv=1e30;
            for(int i=n_seed_members;i<ms.n;i++) if(ms.memb[i]<wv){wv=ms.memb[i];worst=i;}
            if(worst<0) break;
            free(ms.id[worst]); free(ms.emb[worst]);
            ms.id[worst]=ms.id[ms.n-1]; ms.emb[worst]=ms.emb[ms.n-1];
            ms.memb[worst]=ms.memb[ms.n-1]; ms.sal[worst]=ms.sal[ms.n-1];
            ms.n--;
        }
    }

    int M = ms.n;
    /* ── final centroid over all embedded members ── */
    float* centroid=calloc((size_t)dim,sizeof(float));
    int nemb=0; int* eidx=malloc((size_t)M*sizeof(int));
    for(int i=0;i<M;i++) if(ms.emb[i]){ eidx[nemb++]=i;
        for(int d=0;d<dim;d++) centroid[d]+=ms.emb[i][d]; }
    if(nemb){ for(int d=0;d<dim;d++) centroid[d]/=(float)nemb; }

    /* ── radius + per-member cosine distance to centroid ── */
    double total_var=0;
    double* distc=calloc((size_t)M,sizeof(double));
    /* normalize centroid direction for cosine distances */
    float* cdir=malloc((size_t)dim*sizeof(float));
    int have_cdir = (nemb>0 && normcopy(centroid,dim,cdir)==0);
    for(int i=0;i<M;i++){
        if(ms.emb[i] && have_cdir){
            double cs=dotf(ms.emb[i],cdir,dim); if(cs>1)cs=1; if(cs<-1)cs=-1;
            distc[i]=1.0-cs;
        } else distc[i]=-1.0; /* unknown */
    }
    /* variance = mean squared Euclid distance of normalized embs to centroid */
    for(int j=0;j<nemb;j++){
        int i=eidx[j]; double s=0;
        for(int d=0;d<dim;d++){ double df=(double)ms.emb[i][d]-centroid[d]; s+=df*df; }
        total_var+=s;
    }
    if(nemb) total_var/=nemb;
    double radius=sqrt(total_var>0?total_var:0);

    /* ── principal axes via dual PCA (Jacobi on the m×m Gram of centered embs) ── */
    int n_axes=0; GeoAxis* axes=NULL;
    if(nemb>=2 && nemb<=GEO_EIG_CAP){
        int m=nemb;
        /* centered, row-major m×dim */
        float* Xc=malloc((size_t)m*dim*sizeof(float));
        for(int j=0;j<m;j++){ int i=eidx[j];
            for(int d=0;d<dim;d++) Xc[(size_t)j*dim+d]=ms.emb[i][d]-centroid[d]; }
        double* G=malloc((size_t)m*m*sizeof(double));
        for(int a=0;a<m;a++) for(int b=a;b<m;b++){
            double s=0; for(int d=0;d<dim;d++) s+=(double)Xc[(size_t)a*dim+d]*Xc[(size_t)b*dim+d];
            G[a*m+b]=s; G[b*m+a]=s;
        }
        double* w=malloc((size_t)m*sizeof(double));
        double* V=malloc((size_t)m*m*sizeof(double));
        jacobi_sym(G,m,w,V);
        /* sort eigenvalue indices descending */
        int* ord=malloc((size_t)m*sizeof(int));
        for(int i=0;i<m;i++) ord[i]=i;
        for(int a=0;a<m;a++) for(int b=a+1;b<m;b++) if(w[ord[b]]>w[ord[a]]){int t=ord[a];ord[a]=ord[b];ord[b]=t;}
        int keep=P.top_axes; if(keep>m) keep=m; if(keep<0) keep=0;
        axes=calloc((size_t)keep,sizeof(GeoAxis));
        for(int t=0;t<keep;t++){
            int c=ord[t];
            double lam=w[c]; if(lam<0) lam=0;
            double eigcov = lam/(double)(m-1);   /* covariance eigenvalue */
            /* principal axis in R^dim: a = Xc^T u_c, then unit-normalize */
            float* ax=calloc((size_t)dim,sizeof(float));
            for(int d=0;d<dim;d++){ double s=0;
                for(int j=0;j<m;j++) s+=(double)V[j*m+c]*Xc[(size_t)j*dim+d];
                ax[d]=(float)s; }
            double nn=0; for(int d=0;d<dim;d++) nn+=(double)ax[d]*ax[d]; nn=sqrt(nn);
            if(nn>1e-12) for(int d=0;d<dim;d++) ax[d]=(float)(ax[d]/nn);
            axes[t].axis=ax; axes[t].extent=sqrt(eigcov);
            n_axes++;
        }
        free(Xc); free(G); free(w); free(V); free(ord);
    }

    /* ── skeleton: internal hebb edges among members + centrality gradient ── */
    GeoEdge* edges=NULL; int n_edges=0, cap_e=0;
    double* centrality=calloc((size_t)M,sizeof(double));
    int* degree=calloc((size_t)M,sizeof(int));
    /* co-registration accumulators */
    double cr_n=0, cr_sx=0, cr_sy=0, cr_sxx=0, cr_syy=0, cr_sxy=0;
    for(int i=0;i<M;i++){
        StoreEdge* es=NULL; size_t ne=0;
        if(store_get_edges_from(store, ms.id[i], &es, &ne)==0 && es){
            for(size_t e=0;e<ne;e++){
                if(es[e].tombstoned || es[e].inhibitory) continue;
                int j=ms_find(&ms, es[e].to_id);
                if(j<0 || j<=i) continue; /* internal, undirected, i<j only */
                double w=eff_w(es[e].weight, es[e].hebb);
                if(w < P.edge_min_weight) continue;
                if(n_edges==cap_e){ cap_e=cap_e?cap_e*2:32;
                    GeoEdge* t=realloc(edges,(size_t)cap_e*sizeof(GeoEdge)); if(!t) break; edges=t; }
                edges[n_edges].a=(uint32_t)i; edges[n_edges].b=(uint32_t)j;
                edges[n_edges].eff_weight=w; edges[n_edges].hebb=es[e].hebb;
                n_edges++;
                centrality[i]+=w; centrality[j]+=w; degree[i]++; degree[j]++;
                /* co-registration: relational strength vs semantic proximity */
                if(ms.emb[i] && ms.emb[j]){
                    double cs=dotf(ms.emb[i],ms.emb[j],dim); if(cs>1)cs=1; if(cs<-1)cs=-1;
                    double x=w, y=cs;
                    cr_n++; cr_sx+=x; cr_sy+=y; cr_sxx+=x*x; cr_syy+=y*y; cr_sxy+=x*y;
                }
            }
        }
        store_edges_free(es,ne);
    }
    double co_reg=0;
    if(cr_n>=2){
        double cov=cr_sxy - cr_sx*cr_sy/cr_n;
        double vx=cr_sxx - cr_sx*cr_sx/cr_n, vy=cr_syy - cr_sy*cr_sy/cr_n;
        if(vx>1e-12 && vy>1e-12) co_reg=cov/sqrt(vx*vy);
    }

    /* ── k-core: peel members by internal degree to get core numbers ── */
    int* core=calloc((size_t)M,sizeof(int));
    {
        int* deg=malloc((size_t)M*sizeof(int));
        int* removed=calloc((size_t)M,sizeof(int));
        for(int i=0;i<M;i++) deg[i]=degree[i];
        int level=0, remaining=M;
        while(remaining>0){
            int progressed=0;
            for(int i=0;i<M;i++){
                if(!removed[i] && deg[i]<=level){
                    core[i]=level; removed[i]=1; remaining--; progressed=1;
                    /* decrement neighbors' working degree */
                    for(int e=0;e<n_edges;e++){
                        int o=-1;
                        if((int)edges[e].a==i && !removed[edges[e].b]) o=edges[e].b;
                        else if((int)edges[e].b==i && !removed[edges[e].a]) o=edges[e].a;
                        if(o>=0) deg[o]--;
                    }
                }
            }
            if(!progressed) level++;
        }
        free(deg); free(removed);
    }
    int k_core=0; for(int i=0;i<M;i++) if(core[i]>k_core) k_core=core[i];

    /* hub = highest centrality (tie-break salience) */
    int hub=-1; double hv=-1;
    for(int i=0;i<M;i++){ double v=centrality[i]+1e-6*ms.sal[i];
        if(v>hv){hv=v;hub=i;} }
    if(hub<0) hub=0;

    /* ── assemble descriptor ── */
    GeoDescriptor* g=calloc(1,sizeof(GeoDescriptor));
    g->dim=dim;
    g->hub_id = strdup(ms.id[hub]);
    g->centroid = centroid;      /* transfer ownership */
    g->n_axes=n_axes; g->axes=axes;
    g->total_variance=total_var; g->radius=radius;
    g->n_members=M; g->n_embedded=nemb;
    g->members=calloc((size_t)M,sizeof(GeoMember));
    for(int i=0;i<M;i++){
        g->members[i].id=strdup(ms.id[i]);
        g->members[i].membership=ms.memb[i];
        g->members[i].centrality=centrality[i];
        g->members[i].salience=ms.sal[i];
        g->members[i].core=core[i];
        g->members[i].dist_centroid=distc[i];
        g->members[i].embedded=ms.emb[i]?1:0;
    }
    g->n_edges=n_edges; g->edges=edges;
    g->k_core=(P.kcore_k>0?P.kcore_k:k_core);
    g->co_registration=co_reg;

    free(centrality); free(degree); free(core); free(distc); free(eidx);
    free(cdir);
    ms_free(&ms);
    return g;
}

void engram_geo_free(GeoDescriptor* g){
    if(!g) return;
    free(g->hub_id); free(g->centroid);
    for(int i=0;i<g->n_axes;i++) free(g->axes[i].axis);
    free(g->axes);
    for(int i=0;i<g->n_members;i++) free(g->members[i].id);
    free(g->members); free(g->edges);
    free(g);
}