#include "kernels.h"
#include <math.h>

typedef struct {
    int stage;
    double begin, next;
} SignalInterval;

/* Both queries must construct exactly the same absolute boundary. In
 * particular, fmod(t-start,period) can still be below a stage offset at the
 * double returned by the event query (e.g. 44 s for periods of .8+10 s). */
static double signal_boundary(double start, double offset, double cycle, double period) {
    return cycle==0 ? start+offset : (start+offset)+cycle*period;
}

static int signal_cycle(double t, double start, double period, double *current, double *next) {
    double lo=fmax(0,floor((t-start)/period));
    if (!isfinite(lo)) return 0;
    double hi=fmax(lo+1,nextafter(lo,INFINITY));
    if (signal_boundary(start,0,lo,period)>t) {
        hi=lo; lo=fmax(0,hi-1);
        if (signal_boundary(start,0,lo,period)>t) lo=0;
    } else {
        /* Usually hi=lo+1 already brackets t. At coarse time resolution,
         * several cycles can round to the same double: bracket/bisect them
         * rather than advancing one cycle at a time or shifting t. */
        while (signal_boundary(start,0,hi,period)<=t) {
            lo=hi; hi=fmax(2*hi,nextafter(hi,INFINITY));
            if (!isfinite(hi)) return 0;
        }
    }
    for (;;) {
        double mid=floor(lo+(hi-lo)/2);
        if (mid<=lo || mid>=hi) break;
        if (signal_boundary(start,0,mid,period)<=t) lo=mid;
        else hi=mid;
    }
    *current=lo; *next=hi;
    return 1;
}

static SignalInterval signal_interval(double t, double start, int stages, int cyclic,
                                      const double *data) {
    if (t<start) return (SignalInterval){-1,start,start};
    double offsets[9]={0};
    for (int i=0;i<stages;i++) offsets[i+1]=offsets[i]+data[16+i];
    double period=offsets[stages], cycle=0, next_cycle=0;
    cyclic=cyclic && period>0 && isfinite(period);
    if (cyclic && !signal_cycle(t,start,period,&cycle,&next_cycle))
        return (SignalInterval){-2,NAN,NAN};
    for (int i=0;i<stages;i++) {
        /* A cycle's last endpoint is the next cycle's first boundary,
         * never a separately rounded (start+period)+cycle*period. */
        double right=cyclic && offsets[i+1]==period ? signal_boundary(start,0,next_cycle,period) :
                     signal_boundary(start,offsets[i+1],cycle,period);
        if (t<right)
            return (SignalInterval){i,signal_boundary(start,offsets[i],cycle,period),right};
    }
    return (SignalInterval){stages,0,INFINITY};
}

double native_signal(double t, double start, int stages, int cyclic, const double *data) {
    SignalInterval interval=signal_interval(t,start,stages,cyclic,data);
    if (interval.stage==-2) return NAN;
    if (interval.stage==-1) return data[0];
    if (interval.stage==stages) return data[8+stages-1];
    int i=interval.stage;
    double d=data[16+i];
    if (d<=0) return data[8+i];
    double fraction=fmin(1,fmax(0,(t-interval.begin)/d));
    if (fraction==0) return data[i];
    if (fraction==1) return data[8+i];
    return data[i]+fraction*(data[8+i]-data[i]);
}

double native_signal_break(double t, double end, double start, int stages,
                           int cyclic, const double *data) {
    SignalInterval interval=signal_interval(t,start,stages,cyclic,data);
    return interval.stage==-2 ? NAN : fmin(end,interval.next);
}