src/sd_hist.c

   1 /* copyright 2016 Apache 2 sddekit authors */
   2
   3 #include "sddekit.h"
   4
   5 typedef struct hist_data
   6 {
   7         /*               nu+1   nu    nu    nu   nd, maxvi */
   8         uint32_t nd, nu, *lim, *len, *pos, *uvi, *vi, *vi2i, maxvi;
   9         /*   sum(len)  nu     nd                */
  10         double *buf, *maxd, *del, dt, t;
  11 } hist_data;
  12
  13 uint32_t get_maxvi(sd_hist *h) { return ((hist_data*) h->ptr)->maxvi; }
  14 uint32_t get_vi2i(sd_hist *h, uint32_t vi) { return ((hist_data*) h->ptr)->vi2i[vi]; }
  15
  16 static void update_time(sd_hist *hist, double new_t)
  17 {
  18         uint32_t i, n_steps;
  19         hist_data *h = hist->ptr;
  20
  21         /* the current time must always be contained between
  22          * pos and pos-1 in the buffer
  23          *
  24          * TODO handle case of irregular time step where new_t could
  25          * be several h->dt ahead, and we need to fill in the buffer
  26          * correctly.
  27          */
  28         n_steps = 0;
  29         while ((h->t + h->dt) <= new_t)
  30         {
  31                 h->t += h->dt;
  32                 n_steps++;
  33         }
  34
  35         /* if no steps to take, return early */
  36         if (n_steps==0)
  37                 return;
  38
  39         /* update positions */
  40         for (i=0; i<h->nu; i++)
  41         {
  42                 if (h->pos[i] < n_steps)
  43                         h->pos[i] = h->len[i] - n_steps;
  44                 else
  45                         h->pos[i] -= n_steps;
  46         }
  47 }
  48
  49 static void hist_free(sd_hist *hist)
  50 {
  51         hist_data *h = hist->ptr;
  52         sd_free(h->uvi);
  53         sd_free(h->del);
  54         sd_free(h->vi);
  55         sd_free(h->maxd);
  56         sd_free(h->lim);
  57         sd_free(h->len);
  58         sd_free(h->pos);
  59         sd_free(h->vi2i);
  60         sd_free(h->buf);
  61         sd_free(h);
  62         sd_free(hist);
  63 }
  64
  65 static sd_stat fill(sd_hist *hist, sd_hfill *hf)
  66 {
  67         uint32_t i, j, ui, o, n, *vi;
  68         double *t;
  69         char *errmsg;
  70         hist_data *h = hist->ptr;
  71         errmsg = NULL;
  72         t = NULL;
  73         vi = NULL;
  74
  75         n = h->lim[h->nu];
  76         if ((t = sd_malloc (sizeof(double) * n))==NULL ||
  77                 (vi = sd_malloc (sizeof(uint32_t) * n))==NULL) {
  78                 errmsg = "failed to allocate memory for evaluating hist fill.";
  79                 goto end;
  80         }
  81
  82         /* expand indices per buffer element */
  83         for (i=0; i<h->nu; i++)
  84         {
  85                 ui = h->uvi[i];
  86                 for (j=h->lim[i]; j<h->lim[i+1]; j++)
  87                         vi[j] = ui;
  88         }
  89
  90         /* evaluate time per buffer element */
  91         for (i=0; i<h->nu; i++)
  92         {
  93                 for (j=0; j<(h->len[i]-1); j++)
  94                 {
  95                         o = h->pos[i];     /* current position in buffer */
  96                         o += j;            /* time step through buffer */
  97                         o %= h->len[i];    /* wrap around on section length */
  98                         o += h->lim[i];    /* offset to start of section */
  99                         t[o] = h->t - j * h->dt;
 100                 }
 101                 j = h->len[i] - 1;
 102                 o = h->pos[i];     /* current position in buffer */
 103                 o += j;            /* time step through buffer */
 104                 o %= h->len[i];    /* wrap around on section length */
 105                 o += h->lim[i];    /* offset to start of section */
 106                 t[o] = h->t + h->dt; /* last point is next grid point */
 107         }
 108
 109         if (hf->apply(hf, n, t, vi, h->buf) != SD_OK)
 110                 errmsg = "history fill function failed.";
 111
 112 end:
 113         if (t!=NULL) sd_free(t);
 114         if (vi!=NULL) sd_free(vi);
 115
 116         return errmsg == NULL ? SD_OK : SD_ERR;
 117 }
 118
 119 static void get(sd_hist *hist, double t, double *aff)
 120 {
 121         uint32_t i;
 122         hist_data *h = hist->ptr;
 123
 124         if (h==NULL)
 125                 return;
 126
 127         for (i = 0; i < h->nd; i++)
 128         {
 129                 uint32_t ui = h->vi2i[h->vi[i]] /* unique variable index */
 130                        ,  p = h->pos[ui]        /* rel pos in ui's var buf */
 131                        ,  l = h->len[ui]        /* len of ui's var buf */
 132                        ,  o = h->lim[ui];       /* offset of ui's buf in h->buf */
 133
 134                 /* TODO the following dt is not sol dt or hist dt, find better name */
 135                 double dt = (t - h->del[i]) - (h->t - (l - 2)*h->dt);
 136
 137                 /* relative buffer indices */
 138                 int64_t i0 = (int64_t) ceil((l - 2) - dt / h->dt);
 139                 if (i0 < 0)
 140                         i0 += l;
 141                 int64_t i1 = i0 > 0 ? ((uint32_t) i0) - 1 : l - 1;
 142
 143                 /* absolute buffer indices */
 144                 uint32_t i0_ = (p + i0) % l + o
 145                        , i1_ = (p + i1) % l + o;
 146
 147 #ifdef SDDEBUG
 148                 if ((i0_ < h->lim[ui]) || (i0_ >= h->lim[ui+1]))
 149                         sd_log_debug("[sd_hist_get] oob: i0_=%d not in [%d, %d) at %s:%d\n",
 150                                      i0_, h->lim[ui], h->lim[ui+1], __FILE__, __LINE__ );
 151                 if ((i1_ < h->lim[ui]) || (i1_ >= h->lim[ui+1]))
 152                         sd_log_debug("[sd_hist_get] oob: i0_=%d not in [%d, %d) at %s:%d\n",
 153                                      i1_, h->lim[ui], h->lim[ui+1], __FILE__, __LINE__ );
 154 #endif
 155
 156                 double y0 = h->buf[i0_] /* consider sse read */
 157                      , y1 = h->buf[i1_]
 158                      ,  m = (y1 - y0) / h->dt
 159                      , dx = (t - h->del[i]) - (h->t - i0*h->dt);
 160
 161                 aff[i] = m * dx + y0;
 162         }
 163 }
 164
 165 static void set(sd_hist *hist, double t, double *eff)
 166 {
 167         uint32_t i, i0, i1;
 168         double x0, dx, dt;
 169         hist_data *h = hist->ptr;
 170
 171         update_time(hist, t);
 172
 173         for (i=0; i<h->nu; i++)
 174         {
 175                 i0 = h->pos[i];
 176                 i1 = i0 ? i0 - 1 : h->len[i] - 1;
 177                 i0 += h->lim[i];
 178                 i1 += h->lim[i];
 179 #ifdef SDDEBUG
 180         if ((i0 < h->lim[i]) || (i0 > h->lim[i+1]))
 181             fprintf(stderr, "[sd_hist_set] t=%.3f ui=%d, i0=%d not in [%d,%d) %s:%d\n",
 182                     t, i, i0, h->lim[i], h->lim[i+1], __FILE__, __LINE__);
 183         if ((i1 < h->lim[i]) || (i1 > h->lim[i+1]))
 184             fprintf(stderr, "[sd_hist_set] t=%.3f ui=%d, i1=%d not in [%d,%d) %s:%d\n",
 185                     t, i, i1, h->lim[i], h->lim[i+1], __FILE__, __LINE__);
 186 #endif
 187                 x0 = h->buf[i0];
 188                 dt = t - h->t;
 189
 190 #ifdef SDDEBUG
 191                 if (dt < 0)
 192                         sd_log_debug( "[sd_hist] unhandled dt<0 at %s:%d\n", __FILE__, __LINE__ );
 193 #endif
 194
 195                 if (dt > 0) {
 196                         /* extrapolate from (x(h->t), x(t)) to next grid point*/
 197                         dx = eff[h->vi2i[h->uvi[i]]] - h->buf[i0];
 198                         h->buf[i1] = (dx / dt) * h->dt + x0;
 199                 } else {
 200                         /* reset grid point value */
 201                         h->buf[i0] = eff[h->vi2i[h->uvi[i]]];
 202                 }
 203         }
 204 }
 205
 206 static uint32_t nbytes(sd_hist *hist)
 207 {
 208         uint32_t nb;
 209         hist_data *h = hist->ptr;
 210         nb = sizeof(sd_hist) + sizeof(hist_data);
 211         nb += sizeof(uint32_t) * ((h->nu+1) + 3*h->nu + h->nd + h->maxvi);
 212         nb += sizeof(double) * (h->lim[h->nu] + h->nu + h->nd);
 213         return nb;
 214 }
 215
 216 static uint32_t get_nu(sd_hist *h) {
 217         return ((hist_data*) h->ptr)->nu;
 218 }
 219
 220 static double get_buf_lin(sd_hist *hist, uint32_t index)
 221 {
 222         hist_data *h = hist->ptr;
 223 #ifdef SDDEBUG
 224         if (index >= h->lim[h->nu])
 225                 sd_log_debug( "[hist->get_buf_lin] oob index=%d not in [0, %d)\n", index, h->lim[h->nu] );
 226 #endif
 227         return h->buf[index];
 228 }
 229
 230 static double get_t(sd_hist *h) {
 231         return ((hist_data*) h->ptr)->t;
 232 }
 233
 234 static double get_dt(sd_hist *h) {
 235         return ((hist_data*) h->ptr)->dt;
 236 }
 237
 238 static uint32_t get_lim(sd_hist *h, uint32_t i) {
 239         return ((hist_data*) h->ptr)->lim[i];
 240 }
 241
 242 static uint32_t get_len(sd_hist *h, uint32_t i) {
 243         return ((hist_data*) h->ptr)->len[i];
 244 }
 245
 246 static uint32_t get_nd(sd_hist *h) {
 247         return ((hist_data*) h->ptr)->nd;
 248 }
 249
 250 static uint32_t get_pos(sd_hist *h, uint32_t i) {
 251         return ((hist_data*) h->ptr)->pos[i];
 252 }
 253
 254 static uint32_t get_uvi(sd_hist *h, uint32_t i) {
 255         return ((hist_data*) h->ptr)->uvi[i];
 256 }
 257
 258 static uint32_t get_vi(sd_hist *h, uint32_t i) {
 259         return ((hist_data*) h->ptr)->vi[i];
 260 }
 261
 262 static double get_maxd(sd_hist *h, uint32_t i) {
 263         return ((hist_data*) h->ptr)->maxd[i];
 264 }
 265
 266 static bool buf_is_null(sd_hist *h) {
 267         return ((hist_data*) h->ptr)->buf == NULL;
 268 }
 269
 270 static double get_vd(sd_hist *h, uint32_t i) {
 271         return ((hist_data*) h->ptr)->del[i];
 272 }
 273
 274 static sd_hist sd_hist_defaults = {
 275         .ptr = NULL,
 276         .get_maxvi = &get_maxvi,
 277         .get_vi2i = &get_vi2i,
 278         .get_nu = &get_nu,
 279         .free = &hist_free,
 280         .fill = &fill,
 281         .get = &get,
 282         .set = &set,
 283         .nbytes = &nbytes,
 284         .get_buf_lin = &get_buf_lin,
 285         .get_nd = &get_nd,
 286         .get_t = &get_t,
 287         .get_dt = &get_dt,
 288         .get_lim = &get_lim,
 289         .get_len = &get_len,
 290         .get_pos = &get_pos,
 291         .get_uvi = &get_uvi,
 292         .get_maxd = &get_maxd,
 293         .get_vi = &get_vi,
 294         .get_vd = &get_vd,
 295         .buf_is_null = &buf_is_null
 296 };
 297
 298 static sd_stat setup_buffer_structure(hist_data *h, double dt)
 299 {
 300         uint32_t i, j, ui;
 301         double maxd;
 302         char *errmsg;
 303         /* alloc */
 304         if (
 305                 (h->maxd = sd_malloc (sizeof(double) * h->nu))==NULL ||
 306                 (h->lim = sd_malloc (sizeof(double) * (h->nu + 1)))==NULL ||
 307                 (h->len = sd_malloc (sizeof(double) * h->nu))==NULL ||
 308                 (h->pos = sd_malloc (sizeof(double) * h->nu))==NULL
 309            ) {
 310                 errmsg = "failed to alloc internal storage.";
 311                 goto fail;
 312         }
 313         /* vi2i requires max(uvi) then filling in vi2i[ui]=i */
 314         h->maxvi = 0;
 315         for (i=0; i<h->nu; i++)
 316                 if (h->uvi[i] > h->maxvi)
 317                         h->maxvi = h->uvi[i];
 318         /* alloc */
 319         if ((h->vi2i = sd_malloc (sizeof(uint32_t) * (h->maxvi + 1)))==NULL) {
 320                 errmsg = "failed to alloc internal storage.";
 321                 goto fail;
 322         }
 323         /* set up vi2i, maxd len pos lim */
 324         for (i=0; i<h->nu; i++)
 325         {
 326                 ui = h->uvi[i];
 327                 h->vi2i[ui] = i;
 328                 maxd = 0.0;
 329                 for (j=0; j<h->nd; j++)
 330                         if (h->vi[j]==ui && h->del[j]>maxd)
 331                                 maxd = h->del[j];
 332                 h->maxd[i] = maxd;
 333                 h->len[i] = (uint32_t) ceil(maxd / dt) + 2;
 334                 h->pos[i] = 0;
 335                 if (i==0)
 336                         h->lim[i] = 0;
 337                 else
 338                         h->lim[i] = h->lim[i-1] + h->len[i-1];
 339         }
 340         h->lim[h->nu] = h->lim[h->nu-1] + h->len[h->nu-1];
 341         return SD_OK;
 342 fail:
 343         if (h->maxd!=NULL) sd_free(h->maxd);
 344         if (h->lim!=NULL) sd_free(h->lim);
 345         if (h->len!=NULL) sd_free(h->len);
 346         if (h->pos!=NULL) sd_free(h->pos);
 347         if (h->vi2i!=NULL) sd_free(h->vi2i);
 348         sd_err(errmsg);
 349         return SD_ERR;
 350 }
 351
 352 sd_hist *
 353 sd_hist_new_default(uint32_t nd, uint32_t *vi, double *vd, double t0, double dt)
 354 {
 355         sd_hist *hist;
 356         hist_data *h = NULL, zh = { 0 };
 357         if ((hist = sd_malloc(sizeof(sd_hist))) == NULL
 358                 || (h = hist->ptr = sd_malloc(sizeof(hist_data))) == NULL
 359                 || (*h = zh, (h->nd = nd) < 1)
 360                 || sd_util_uniqi(nd, vi, &(h->nu), &(h->uvi)) != SD_OK
 361                 || (h->del = sd_malloc(sizeof(double) * nd)) == NULL
 362                 || (h->vi = sd_malloc(sizeof(uint32_t) * nd)) == NULL
 363                 || (memcpy(h->del, vd, nd * sizeof(double)),
 364                     memcpy(h->vi, vi, nd * sizeof(uint32_t)),
 365                         0)
 366                 || setup_buffer_structure(h, dt) != SD_OK
 367                 || (h->buf = sd_malloc(sizeof(double) * h->lim[h->nu])) == NULL
 368                 )
 369         {
 370                 if (hist != NULL) sd_free(hist);
 371                 if (h != NULL)
 372                 {
 373                         if (h->buf!=NULL) sd_free(h->buf);
 374                         if (h->uvi!=NULL) sd_free(h->uvi);
 375                         if (h->del!=NULL) sd_free(h->del);
 376                         if (h->vi!=NULL) sd_free(h->vi);
 377                 }
 378                 sd_err("number of delays < 1 or memory alloc failed.");
 379                 return NULL;
 380         }
 381         h->dt = dt;
 382         h->t = t0;
 383         *hist = sd_hist_defaults;
 384         hist->ptr = h;
 385         return hist;
 386 }
 387
 388 static sd_stat val_fill_apply(sd_hfill *hf, uint32_t n, double * restrict t,
 389                                                           uint32_t *indices, double * restrict buf)
 390 {
 391         uint32_t i;
 392         double value = *((double*)hf->ptr);
 393         (void)t; (void)indices;
 394         for (i = 0; i < n; i++)
 395                 buf[i] = value;
 396         return SD_OK;
 397 }
 398
 399 static void val_fill_free(sd_hfill *hf)
 400 {
 401         sd_free(hf->ptr);
 402         sd_free(hf);
 403 }
 404
 405 sd_hfill *
 406 sd_hfill_new_val(double val)
 407 {
 408         sd_hfill *hf;
 409         if ((hf=sd_malloc(sizeof(sd_hfill))) == NULL
 410                 || (hf->ptr=sd_malloc(sizeof(double))) == NULL)
 411         {
 412                 sd_err("alloc hfill failed.");
 413                 return hf;
 414         }
 415         *((double*)hf->ptr) = val;
 416         hf->free = &val_fill_free;
 417         hf->apply = &val_fill_apply;
 418         return hf;
 419 }