/* Private implementation included by common.c. All probes use state-only
 * descriptors: no model RHS, property evaluation, mode mutation or reset. */
#if NCONTACTS
#include <float.h>

static double contact_penetration(int j,const double *y) {
    NativeContact c=model_contacts[j];
    double x1=y[c.velocity1+1],x2=y[c.velocity2+1];
    /* Match each lowering's existing force formula, including association. */
    return c.subtract_first ? -(c.gap0+(x2-x1)) : -(c.gap0+x2-x1);
}
static double contact_velocity(int j,const double *y) {
    NativeContact c=model_contacts[j];
    return y[c.velocity1]-y[c.velocity2];
}
static double contact_value(int j,int kind,const double *y) {
    if(kind==2) return contact_velocity(j,y);
    double p=contact_penetration(j,y);
    if(!kind) return p;
    NativeContact c=model_contacts[j];
    double fraction=c.depth>0 ? -expm1(-fmax(p,0)/c.depth) : 1;
    return c.stiffness*p+fraction*c.damping*contact_velocity(j,y);
}
static double contact_locate(NativeRun *r,int j,int kind,double left,double right,
                             double sign,NativeDense dense,void *context) {
    double y[NSTATES];
    for(int k=0;k<60;k++) {
        double mid=left+.5*(right-left);
        if(mid<=left || mid>=right) break;
        r->contact_dense++;r->contact_roots++;
        if(!dense(context,mid,y)) return NAN;
        double g=contact_value(j,kind,y);
        if(!isfinite(g)) return NAN;
        if(sign<0 ? g>=0 : g<=0) right=mid; else left=mid;
    }
    return right;
}
static double contact_bracket(NativeRun *r,int j,int force,double left,double right,
                              double a,double b,NativeDense dense,void *context,int *direction) {
    int slot=2*j+force;
    if(r->contact_direction[slot] && r->contact_last[slot]==left)
        a=r->contact_direction[slot]*DBL_MIN;
    if(a==0 || (a>0 ? b>0 : b<0)) return INFINITY;
    *direction=a<0 ? 1 : -1;
    return contact_locate(r,j,force,left,right,a,dense,context);
}
static double contact_candidate(NativeRun *r,int j,int force,double t,double next,
                                const double *old,const double *trial,
                                NativeDense dense,void *context,int *direction) {
    double a=contact_value(j,force,old),b=contact_value(j,force,trial);
    if(!isfinite(a) || !isfinite(b)) return NAN;
    /* Resolve a same-sign endpoint excursion by splitting at velocity reversal.
     * This assumes resolved steps, not arbitrarily many oscillations per step. */
    if(!force) {
        double va=contact_velocity(j,old),vb=contact_velocity(j,trial);
        if((va<0 && vb>0) || (va>0 && vb<0)) {
            double turn=contact_locate(r,j,2,t,next,va,dense,context),y[NSTATES];
            if(!isfinite(turn)) return NAN;
            r->contact_dense++;
            if(!dense(context,turn,y)) return NAN;
            double g=contact_value(j,force,y);
            if(g==0 && ((a<0 && b<0) || (a>0 && b>0))) return INFINITY;
            double found=contact_bracket(r,j,force,t,turn,a,g,dense,context,direction);
            if(isfinite(found) || isnan(found)) return found;
            return contact_bracket(r,j,force,turn,next,g,b,dense,context,direction);
        }
    }
    return contact_bracket(r,j,force,t,next,a,b,dense,context,direction);
}
static int contact_candidates(NativeRun *r,double t,double next,const double *old,const double *trial,
                              NativeDense dense,void *context,double *when,int *indices,int *kinds,int *count) {
    for(int j=0;j<NCONTACTS;j++) {
        r->contact_checks++;
        for(int force=0;force<2;force++) {
            if(force && (model_contacts[j].signed_force==1 || contact_penetration(j,old)<=0)) continue;
            int direction=0;
            double at=contact_candidate(r,j,force,t,next,old,trial,dense,context,&direction);
            if(isnan(at)) return 0;
            if(isfinite(at)) {
                if(force) {
                    double y[NSTATES];r->contact_dense++;
                    if(!dense(context,at,y)) return 0;
                    if(contact_penetration(j,y)<=0) continue;
                }
                int n=(*count)++;
                when[n]=at;indices[n]=j;kinds[n]=force?-3:-2;
                r->contact_pending[2*j+force]=direction;
            }
        }
    }
    return 1;
}
static void contact_commit(NativeRun *r,double t,const double *y,
                           const double *when,const int *indices,const int *kinds,int count) {
    for(int i=0;i<count;i++) {
        if(kinds[i]>-2 || when[i]!=t) continue;
        int j=indices[i],force=kinds[i]==-3,slot=2*j+force;
        r->contact_last[slot]=t;r->contact_direction[slot]=r->contact_pending[slot];
        if(force) r->contact_clipping++; else r->contact_transitions++;
        fprintf(stderr,"{\"phase\":\"mechanical-event\",\"kind\":\"%s\",\"index\":%d,\"time\":%.17g,\"direction\":%d,\"penetration\":%.17g,\"relativeVelocity\":%.17g}\n",
            force?"force-clip":"lstp-contact",j,t,r->contact_direction[slot],
            contact_penetration(j,y),contact_velocity(j,y));
    }
}
#endif