sk_hist.c

   1 /* Apache 2.0 INS-AMU 2015 */
   2
   3 #include "stdlib.h"
   4 #include "string.h"
   5 #include "math.h"
   6
   7 #include "sk_util.h"
   8 #include "sk_hist.h"
   9
  10
  11 static void setup_buffer_structure(sk_hist *h)
  12 {
  13         int i, j, ui;
  14         double maxd;
  15
  16         SK_MALLOCHECK(h->maxd = malloc (sizeof(double) * h->nu));
  17         SK_MALLOCHECK(h->lim = malloc (sizeof(double) * (h->nu + 1)));
  18         SK_MALLOCHECK(h->len = malloc (sizeof(double) * h->nu));
  19         SK_MALLOCHECK(h->pos = malloc (sizeof(double) * h->nu));
  20
  21         /* vi2i requires max(uvi) then filling in vi2i[ui]=i */
  22         h->maxvi = 0;
  23         for (i=0; i<h->nu; i++)
  24                 if (h->uvi[i] > h->maxvi)
  25                         h->maxvi = h->uvi[i];
  26
  27         SK_MALLOCHECK(h->vi2i = malloc (sizeof(int) * (h->maxvi + 1)));
  28
  29         for (i=0; i<h->nu; i++)
  30         {
  31                 ui = h->uvi[i];
  32                 h->vi2i[ui] = i;
  33                 maxd = 0.0;
  34                 /* TODO double check */
  35                 for (j=0; j<h->nd; j++)
  36                         if (h->vi[j]==ui && h->del[j]>maxd)
  37                                 maxd = h->del[j];
  38                 h->maxd[i] = maxd;
  39                 h->len[i] = ceil(maxd / h->dt) + 2;
  40                 h->pos[i] = 0;
  41                 if (i==0)
  42                         h->lim[i] = 0;
  43                 else
  44                         h->lim[i] = h->pos[i-1] + h->len[i-1];
  45         }
  46         h->lim[h->nu] = h->lim[h->nu-1] + h->len[h->nu-1];
  47 }
  48
  49 void sk_hist_init(sk_hist *h, int nd, int *vi, double *vd, double t0, double dt)
  50 {
  51         h->dt = dt;
  52         h->t = t0;
  53         h->nd = nd;
  54
  55         /* identify unique delayed variable indices
  56          * NB: this allocates h->uci
  57          */
  58         sk_util_uniqi(nd, vi, &(h->nu), &(h->uvi));
  59
  60         /* alloc & copy the delay values */
  61         SK_MALLOCHECK(h->del = malloc (sizeof(double) * nd));
  62         memcpy(h->del, vd, nd * sizeof(double));
  63
  64         /* and indices */
  65         SK_MALLOCHECK(h->vi = malloc (sizeof(int) * nd));
  66         memcpy(h->vi, vi, nd * sizeof(int));
  67
  68         /* setup maxd, off, len, pos */
  69         setup_buffer_structure(h);
  70
  71         /* alloc buf */
  72         SK_MALLOCHECK(h->buf = malloc (sizeof(double) * h->lim[h->nu]));
  73 }
  74
  75 void sk_hist_free(sk_hist *h)
  76 {
  77         free(h->uvi);
  78         free(h->del);
  79         free(h->vi);
  80         free(h->maxd);
  81         free(h->lim);
  82         free(h->len);
  83         free(h->pos);
  84     free(h->vi2i);
  85     free(h->buf);
  86 }
  87
  88 void sk_hist_fill(sk_hist *h, sk_hist_filler filler, void *fill_data)
  89 {
  90         int i, j, ui, o, n, *vi;
  91         double *t;
  92
  93         n = h->lim[h->nu];
  94         SK_MALLOCHECK(t = malloc (sizeof(double) * n));
  95         SK_MALLOCHECK(vi = malloc (sizeof(int) * n));
  96
  97         /* expand indices per buffer element */
  98         for (i=0; i<h->nu; i++)
  99         {
 100                 ui = h->uvi[i];
 101                 for (j=h->lim[i]; j<h->lim[i+1]; j++)
 102                         vi[j] = ui;
 103         }
 104
 105         /* evaluate time per buffer element */
 106         for (i=0; i<h->nu; i++)
 107         {
 108                 for (j=0; j<(h->len[i]-1); j++)
 109                 {
 110                         o = h->pos[i];     /* current position in buffer */
 111                         o += j;            /* time step through buffer */
 112                         o %= h->len[i];    /* wrap around on section length */
 113                         o += h->lim[i];    /* offset to start of section */
 114                         t[o] = h->t - j * h->dt;
 115                 }
 116                 j = h->len[i] - 1;
 117                 o = h->pos[i];     /* current position in buffer */
 118                 o += j;            /* time step through buffer */
 119                 o %= h->len[i];    /* wrap around on section length */
 120                 o += h->lim[i];    /* offset to start of section */
 121                 t[o] = h->t + h->dt; /* last point is next grid point */
 122         }
 123
 124         filler(fill_data, n, t, vi, h->buf);
 125
 126         free(t);
 127         free(vi);
 128 }
 129
 130 void sk_hist_get(sk_hist *h, double t, double *c)
 131 {
 132         int i, i0, i1, ui;
 133
 134         for (i=0; i<h->nd; i++)
 135         {
 136                 ui = h->vi2i[h->vi[i]];
 137                 i0 = h->pos[ui] - floor((t - h->del[i] - h->t) / h->dt);
 138                 i0 %= h->len[ui];
 139                 i1 = i0 ? i0 - 1 : h->len[ui]-1;
 140                 i0 += h->lim[ui];
 141                 i1 += h->lim[ui];
 142
 143 #ifdef SKDEBUG
 144         if ((i0 < 0) || (i0 > h->lim[h->nu]))
 145             fprintf(stderr, "[sk_hist] oob i0=%d %s:%d\n", i0, __FILE__, __LINE__);
 146         if ((i1 < 0) || (i1 > h->lim[h->nu]))
 147             fprintf(stderr, "[sk_hist] oob i1=%d %s:%d\n", i1, __FILE__, __LINE__);
 148 #endif
 149                 c[i] = (h->buf[i1] - h->buf[i0]) / h->dt
 150                         * fmod((t + h->del[i]), h->dt) + h->buf[i0];
 151         }
 152 }
 153
 154 static void update_time(sk_hist *h, double new_t)
 155 {
 156         int i, n_steps;
 157
 158         /* the current time must always be contained between
 159          * pos and pos-1 in the buffer
 160          */
 161         n_steps = 0;
 162         while ((h->t + h->dt) < new_t)
 163         {
 164                 h->t += h->dt;
 165                 n_steps++;
 166         }
 167
 168         /* if no steps to take, return early */
 169         if (n_steps==0)
 170                 return;
 171
 172         /* update positions */
 173         for (i=0; i<h->nu; i++)
 174         {
 175                 h->pos[i] -= n_steps;
 176                 if (h->pos[i] < 0)
 177                         h->pos[i] += h->len[i];
 178         }
 179 }
 180
 181 void sk_hist_set(sk_hist *h, double t, double *x)
 182 {
 183         int i, i0, i1;
 184         double x0, dx, dt;
 185
 186         update_time(h, t);
 187
 188         /* extrapolate from (x(h->t), x(t)) to next grid point*/
 189         for (i=0; i<h->nu; i++)
 190         {
 191                 i0 = h->pos[i];
 192                 i1 = i0 ? i0 - 1 : h->len[i] - 1;
 193                 i0 += h->lim[i];
 194                 i1 += h->lim[i];
 195                 x0 = h->buf[i0];
 196                 dt = t - h->t;
 197                 dx = x[h->uvi[i]] - h->buf[i0];
 198                 h->buf[i1] = (dx / dt) * h->dt + x0;
 199         }
 200 }
 201
 202 int sk_hist_nbytes(sk_hist *h)
 203 {
 204         int nb;
 205         nb = sizeof(sk_hist);
 206         nb += sizeof(int) * ((h->nu+1) + 3*h->nu + h->nd + h->maxvi);
 207         nb += sizeof(double) * (h->lim[h->nu] + h->nu + h->nd);
 208         return nb;
 209 }
 210
 211 void sk_hist_zero_filler(void *data, int n, double *t, int *indices, double *buf)
 212 {
 213         int i;
 214         /* suppress unused parameter warnings */
 215         (void) data; (void) n; (void) t; (void) indices;
 216         for (i=0; i<n; i++)
 217                 buf[i] = 0.0;
 218 }