/* Last edited on 2015-01-20 16:56:04 by stolfilocal */

#include <pz_types.h>
#include <pz_proc.h>

#include <affirm.h>
#include <jsmath.h>

#include <math.h>
#include <values.h>
#include <stdio.h>
#include <fenv.h>
#include <fpu_control.h>

double fmax(double x, double y) { return (x > y ? x : y); }
double fmin(double x, double y) { return (x < y ? x : y); }

int pz_round ( double x )
  { fesetround(FE_TONEAREST);
    return rint(x);
  }
    
void pz_reduce_to_period
  ( double *t, /* IO */  
    double tLo,
    double tHi, 
    int *k  /* (OUT) */
  )
  { if ((*t >= tLo) && (*t < tHi))
      { *k = 0; }
    else
      { double T = tHi - tLo;
        affirm(T > 0, "neg period");
        double kk = floor(((*t) - tLo)/T);
        double tt = (*t) - ((double)kk)*T;
        while (tt < tLo){ tt += T; kk++; }
        while (tt >= tHi){ tt -= T; kk--; }
        *t = tt; *k = kk;
      }
  }

#define SqrtTwo (1.4142135623730950488)

double pz_squeeze ( double x, double sigma ) 
  {
    return erf(x/(SqrtTwo*sigma));
  }


double pz_stretch ( double x, double sigma )
  {
    double a, b;
    if ((x <= -1)){ return -INFINITY; }
    if ((x >= +1)){ return +INFINITY; }
    a = -1; b = +1;
    while (x < pz_squeeze(a, sigma))
      { double t = a - 2*(b - a); b = a; a = t; }
    while ( x > pz_squeeze(b, sigma) )
      { double t = b + 2*(b - a); a = b; b = t; }
    while ( b - a > 0.0001e0 )
      { double m = (a + b)/ 2;
        if (x > pz_squeeze(b, sigma)) { a = m; } else { b = m; }
      }
    return (a + b)/2;
  }

double pz_old_squeeze ( double x, double epsilon, double delta )
  { 
    double r = delta*x/epsilon;
    return log(r + sqrt(r*r + 1))/delta;
  }

double pz_old_stretch ( double x, double epsilon, double delta )
  {
    double h = exp(x * delta);
    double r = (h - 1)/(2*h);
    return epsilon*r/delta;
  }

double pz_interpolate
  ( double t, 
    double t0,
    double p0, 
    double t1,
    double p1 
  )
  { 
    double p;
    if ((t1 - t0) == 0)
      {
        double alpha = (t - t0) / (t1 - t0);
        p = alpha * p1 + (1.00 - alpha) * p0;
      }
    else
      { p = 0.5 * p1 + 0.5* p0; }
    return p;
  }

double pz_cubic_interpolate
  ( double t, 
    double t0,
    double p0, 
    double t1,
    double p1,
    double t2,
    double p2,
    double t3,
    double p3 
  )
  { 
    affirm((t0 <= t1 ) && ( t1 <= t2 ) && ( t2 <= t3 ), "time order");
    /* Test for degenerate intervals: */
    double eps = 1.0e-6*(t3 - t0);
    if ((eps == 0) || ((t2-t1) < eps))
      { /* Middle interval is degenerate: */
        return 0.5*p1 + 0.5*p2;
      }
    else if (((t1-t0) < eps) || ((t3-t2) < eps))
      { /* End intervals degenerate: use linear interpolation */
        return pz_interpolate(t, t1, p1, t2, p2);
      }
    double p01 = pz_interpolate(t, t0, p0, t1, p1);
    double p12 = pz_interpolate(t, t1, p1, t2, p2);
    double p23 = pz_interpolate(t, t2, p2, t3, p3);
    double p012 = pz_interpolate(t, t0, p01, t2, p12);
    double p123 = pz_interpolate(t, t1, p12, t3, p23);
    double p0123 = pz_interpolate(t, t0, p012, t3, p123);
    return p0123;
  }

bool_vec_t pz_select_all ( unsigned n )
  {
    bool_vec_t b = bool_vec_new(n);
    int i;
    for (i = 0; i < n; i++) { b.el[i] = TRUE; }
    return b;
  }

double  pz_adjust_unit
  ( double givenUnit, 
    double dev,
    double big 
  )
  {
    double eps, minUnit, newUnit;
    /* Compute desirable unit {eps}: */
    if (big == 0)
      { eps = 1; }
    else if (dev == 0)
      { eps = fmax(big/1.0e-6, 1.0e-10); }
    else
      { eps = fmax(dev/1.0e-6, 1.0e-10); }
    /* Compute minimum unit to avoid overflow: */
    minUnit = big/1.0e+9;
    /* Decide what to do with {givenUnit}: */
    if (givenUnit < minUnit)
      { /* Must increase {givenUnit} to avoid overflow: */
        newUnit = 1;
        while (newUnit/10 >= minUnit){ newUnit = newUnit / 10; }
        while (newUnit < minUnit){ newUnit = newUnit * 10; }
        fprintf(stderr, 
          "warning: unit increased from %g to %g to avoid overflow.\n",
          givenUnit, newUnit
        );
      }
    else if (givenUnit > eps)
      { /* Given unit is too big, try to reduce it. */
        /* Compute smallest power of 10 {p >= minUnit} and {p > eps/10}: */
        newUnit = 1;
        while ((newUnit > eps) && (newUnit/10 >= minUnit))
          { newUnit = newUnit / 10; }
        while ((newUnit <= eps/10 ) && (newUnit < minUnit))
          { newUnit = newUnit * 10; }
        if (newUnit < givenUnit)
          { fprintf(stderr, 
              "warning: reducing unit from %g to %g to reduce quantization errors.\n",
              givenUnit, newUnit
            );
          }
      }
    else
      { newUnit = givenUnit; }
    /* In any case, check whether result is OK: */
    if (newUnit > eps)
      { fprintf(stderr, 
          "warning: possible loss of information dev = %g unit = %g\n",
          dev, newUnit
        );
      }
    return newUnit;
  }

void pz_sort_ok ( int_vec_t *x, bool_vec_t *ok )
  {
    unsigned r, s;
    affirm(x->nel == ok->nel, "wrong size");
    int k;
    for (k = 0; k < x->nel; k++) { x->el[k] = k; }
    r = 0; s = (x->nel - 1);
    while (r < s)
      { /* Invariants: {ok[x[0..r-1]] == TRUE}, {ok[x[s+1..(c.ne - 1)]] == FALSE} */
        /* Look for swappable pair: */
        while ((r < s) && (ok->el[x->el[r]])) { r++; }
        while ((r < s) && (! ok->el[x->el[s]])) { s--; }
        if (r < s)
          { int t = x->el[r]; x->el[r] = x->el[s]; x->el[s] = t;
            r++; s--;
          }
      }
  }

/*
  Copyright © 2001 Universidade Estadual de Campinas (UNICAMP).
  Authors: Helena C. G. Leitão and Jorge Stolfi.
  
  This file can be freely distributed, used, and modified, provided
  that this copyright and authorship notice is preserved, and that any
  modified versions are clearly marked as such.
  
  This software has NO WARRANTY of correctness or applicability for
  any purpose. Neither the authors nor their employers chall be held
  responsible for any losses or damages that may result from its use.
*/