
/* ------ oscils, lphasor, and tablexkt by Istvan Varga (Jan 5 2002) ------ */

#include "cs.h"
#include <math.h>
#define CSOUND_OSCILS_C 1
#include "oscils.h"

/* ------------- set up fast sine generator ------------- */
/* Input args:                                            */
/*   a: amplitude                                         */
/*   f: frequency (-PI - PI)                              */
/*   p: initial phase (0 - PI/2)                          */
/* Output args:                                           */
/*  *x: first output sample                               */
/*  *c, *v: coefficients for calculating next sample as   */
/*          shown below:                                  */
/*            v = v + c * x                               */
/*            x = x + v                                   */
/*          These values are calculated by:               */
/*            x = y[0]                                    */
/*            c = 2.0 * cos(f) - 2.0                      */
/*            v = y[1] - (c + 1.0) * y[0]                 */
/*          where y[0], and y[1] are the first, and       */
/*          second sample of the sine wave to be          */
/*          generated, respectively.                      */
/* -------- written by Istvan Varga, Jan 28 2002 -------- */

static void init_sine_gen (double a, double f, double p,
                           double *x, double *c, double *v)
{
    double  y0, y1;                 /* these should be doubles */

    y0 = sin (p);
    y1 = sin (p + f);
    *x = y0;
    *c = 2.0 * cos (f) - 2.0;
    *v = y1 - *c * y0 - y0;
    /* amp. scale */
    *x *= a; *v *= a;
}


/* -------- oscils set-up -------- */

void oscils_set (OSCILS *p)
{
    int     iflg;

    iflg = (int) (*(p->iflg) + FL(0.5)) & 0x07;   /* check flags */
    if (iflg & 1) return;           /* skip init, nothing to do */
    p->use_double = (iflg & 2 ? 1 : 0);           /* use doubles internally */
    init_sine_gen ((double) *(p->iamp), (double) (*(p->icps) * tpidsr),
                   (double) (*(p->iphs) * TWOPI_F),
                   &(p->xd), &(p->cd), &(p->vd));
    if (!(p->use_double)) {
      p->x = (MYFLT) p->xd;       /* use floats */
      p->c = (MYFLT) p->cd;
      p->v = (MYFLT) p->vd;
    }
}

/* -------- oscils performance -------- */

void oscils (OSCILS *p)
{
    int     nn;
    MYFLT   *ar, x, c, v;
    double  xd, cd, vd;

    /* copy object data to local variables */
    ar = p->ar;

    nn = ksmps;
    if (p->use_double) {            /* use doubles */
      xd = p->xd; cd = p->cd; vd = p->vd;
      while (nn--) {
        *(ar++) = (MYFLT) xd;
        vd += cd * xd;
        xd += vd;
      }
      p->xd = xd; p->vd = vd;
    }
    else {                          /* use floats */
      x = p->x; c = p->c; v = p->v;
      while (nn--) {
        *(ar++) = x;
        v += c * x;
        x += v;
      }
      p->x = x; p->v = v;
    }
}

/* -------- lphasor set-up -------- */

void lphasor_set (LPHASOR *p)
{
    if (*(p->istor) != FL(0.0)) return;               /* nothing to do */

    p->phs = (double) *(p->istrt);                          /* start phase */
    p->lps = (double) *(p->ilps);                           /* loop start */
    p->lpe = (double) *(p->ilpe);                           /* loop end */
    p->loop_mode = (int) (*(p->imode) + FL(0.5)) & 0x03;    /* loop mode */
    if (p->lpe <= p->lps) p->loop_mode = 0;                 /* disable loop */
    p->dir = 1;                                             /* direction */
}

/* -------- lphasor performance -------- */

void lphasor (LPHASOR *p)
{
    int     nn, loop_mode, dir;
    MYFLT   *ar, *xtrns;
    double  trns, phs, lps, lpe, lpt;

    /* copy object data to local variables */
    ar = p->ar; xtrns = p->xtrns;
    phs = p->phs; lps = p->lps; lpe = p->lpe;
    lpt = lpe - lps;
    loop_mode = p->loop_mode;
    trns = (double) *xtrns;

    nn = ksmps;
    while (nn--) {
      if (XINARG1) trns = (double) *(xtrns++);
      *(ar++) = (MYFLT) phs;
      phs += (p->dir ? trns : -trns);
      if (loop_mode) {
        dir = (trns < 0.0 ? !(p->dir) : p->dir);
        if (dir && (phs >= lpe)) {
          phs += lpt * (double) ((int) (lps - phs) / lpt);
          if (loop_mode & 2) {
            phs = lps + lpe - phs;  /* reverse direction */
            p->dir = !(p->dir);
          }
        }
        else if (!dir && (phs <= lps)) {
          phs += lpt * (double) ((int) (lpe - phs) / lpt);
          if (loop_mode & 1) {
            phs = lps + lpe - phs;  /* reverse direction */
            p->dir = !(p->dir);
          }
        }
      }
    }
    /* store phase */
    p->phs = phs;
}

/* -------- tablexkt set-up -------- */

void tablexkt_set (TABLEXKT *p)
{
    p->wsize = (int) (*(p->iwsize) + 0.5);                  /* window size */
    if (p->wsize < 3) {
      p->wsize = 2;
    }
    else {
      p->wsize = ((p->wsize + 2) >> 2) << 2;        /* round to nearest */
      if (p->wsize > 1024) p->wsize = 1024;         /* integer multiply of 4 */
    }
    /* constant for window calculation */
    p->win_fact = (MYFLT) ((1.0 - pow ((double) p->wsize * 0.85172, -0.89624))
                           / (double) ((p->wsize * p->wsize) >> 2));

    p->ndx_scl = (*(p->ixmode) == FL(0.0) ? 0 : 1);         /* index mode */
    p->wrap_ndx = (*(p->iwrap) == FL(0.0) ? 0 : 1);         /* wrap index */
    /* use raw index values without scale / offset */
    if ((*(p->ixoff) != FL(0.0)) || p->ndx_scl) p->raw_ndx = 0;
    else p->raw_ndx = 1;
}

/* -------- tablexkt opcode -------- */

void tablexkt (TABLEXKT *p)
{
    int     i, nn, wsize, wsized2, wrap_ndx, warp;
    double  ndx, d, x, c, v, flen_d, onedpi_d, pidwarp_d;
    long    ndx_i, flen;
    MYFLT   *ar, *xndx, ndx_f, a0, a1, a2, a3, v0, v1, v2, v3, *ftable;
    MYFLT   onedwarp, win_fact;
    FUNC    *ftp;

    /* window size */
    wsize = p->wsize;
    if ((wsize < 2) || (wsize > 1024)) {
      perferror (Str(X_1714,"tablexkt: not initialised")); return;
    }
    wsized2 = wsize >> 1;

    /* check ftable */
    if ((ftp = ftfindp (p->kfn)) == NULL) return;     /* invalid table */
    if ((ftable = ftp->ftable) == NULL) return;
    flen = ftp->flen;               /* table length */
    flen_d = (double) flen;

    /* copy object data to local variables */
    ar = p->ar;
    xndx = p->xndx;
    wrap_ndx = p->wrap_ndx;
    if ((wsize > 4) && (*(p->kwarp) > FL(1.001))) {
      warp = 1;                     /* enable warp */
      onedwarp = FL(1.0) / *(p->kwarp);
      pidwarp_d = PI / (double) *(p->kwarp);
      /* correct window for kwarp */
      x = v = (double) wsized2; x *= x; x = 1.0 / x;
      v *= (double) onedwarp; v -= (double) ((int) v) + 0.5; v *= 4.0 * v;
      win_fact = (MYFLT) (((double) p->win_fact - x) * v + x);
    }
    else {
      warp = 0; onedwarp = FL(0.0); pidwarp_d = 0.0;
      win_fact = p->win_fact;
    }
    onedpi_d = 1.0 / PI;

    nn = ksmps;
    while (nn--) {
      ndx = (double) *xndx;
      if (XINARG1) xndx++;
      /* calculate table index */
      if (!(p->raw_ndx)) {
        ndx += (double) *(p->ixoff);
        if (p->ndx_scl) ndx *= flen_d;
      }
      /* integer and fractional part of table index */
      ndx_i = (long) ndx;
      ndx_f = (MYFLT) (ndx - (double) ndx_i);
      if (ndx_f < FL(0.0)) {
        ndx_f++; ndx_i--;
      }
      /* wrap or limit to allowed range */
      if (wrap_ndx) {
        while (ndx_i >= flen) ndx_i -= flen;
        while (ndx_i < 0) ndx_i += flen;
      }
      else {                        /* limit */
        if (ndx_i < 0) {
          ndx_i = 0; ndx_f = FL(0.0);
        }
        else if (ndx_i >= flen) {
          ndx_i = flen; ndx_f = FL(0.0);
        }
      }
      switch (wsize) {
        case 2:                     /* ---- linear interpolation ---- */
          *ar = ftable[ndx_i];
          if (++ndx_i >= flen) ndx_i = (wrap_ndx ? ndx_i - flen : flen);
          *ar += (ftable[ndx_i] - *ar) * ndx_f;
          break;
        case 4:                     /* ---- cubic interpolation ---- */
          /* sample  0 */
          v1 = ftable[ndx_i];
          /* sample -1 */
          if (ndx_i) {
            v0 = ftable[ndx_i - 1L];
          } else {
            v0 = ftable[(wrap_ndx ? flen - 1L : 0L)];
          }
          /* sample +1 */
          if (++ndx_i >= flen) ndx_i = (wrap_ndx ? ndx_i - flen : flen);
          v2 = ftable[ndx_i];
          /* sample +2 */
          if (++ndx_i >= flen) ndx_i = (wrap_ndx ? ndx_i - flen : flen);
          v3 = ftable[ndx_i];
          a3 = ndx_f * ndx_f; a3--; a3 *= FL(0.1666666667);
          a2 = ndx_f; a2++; a0 = (a2 *= FL(0.5)); a0--;
          a1 = FL(3.0) * a3; a2 -= a1; a0 -= a3; a1 -= ndx_f;
          *ar = (a0 * v0 + a1 * v1 + a2 * v2 + a3 * v3) * ndx_f + v1;
          break;
        default:                    /* ---- sinc interpolation ---- */
          *ar = FL(0.0);        /* clear output */
          ndx = (double) ndx_f;
          ndx_i += (long) (1 - wsized2);
          d = (double) (1 - wsized2) - ndx;
          if (warp) {           /* ---- warp enabled ---- */
            init_sine_gen (onedpi_d, pidwarp_d, pidwarp_d * d, &x, &c, &v);
            /* samples -(window size / 2 - 1) to -1 */
            i = wsized2 - 1;
            do {
              a1 = (MYFLT) d; a1 = FL(1.0) - a1 * a1 * win_fact;
              a1 = a1 * a1 / (MYFLT) d;
              *ar += ftable[(ndx_i < 0L ? (wrap_ndx ? ndx_i + flen : 0L)
                                          : ndx_i)] * (MYFLT) x * a1;
              ndx_i++;
              d++; v += c * x; x += v;
            } while (--i);
            /* sample 0 */
            /* avoid division by zero */
            if (ndx < 0.00003) *ar += onedwarp * ftable[ndx_i];
            else {
              a1 = (MYFLT) d; a1 = FL(1.0) - a1 * a1 * win_fact;
              a1 = a1 * a1 / (MYFLT) d;
              *ar += (MYFLT) x * a1 * ftable[ndx_i];
            }
            d++; v += c * x; x += v;
            if (++ndx_i >= flen) ndx_i = (wrap_ndx ? ndx_i - flen : flen);
            /* sample 1 */
            /* avoid division by zero */
            if (ndx > 0.99997) *ar += onedwarp * ftable[ndx_i];
            else {
              a1 = (MYFLT) d; a1 = FL(1.0) - a1 * a1 * win_fact;
              a1 = a1 * a1 / (MYFLT) d;
              *ar += (MYFLT) x * a1 * ftable[ndx_i];
            }
            d++; v += c * x; x += v;
            if (++ndx_i >= flen) ndx_i = (wrap_ndx ? ndx_i - flen : flen);
            /* samples 2 to (window size / 2) */
            i = wsized2 - 1;
            do {
              a1 = (MYFLT) d; a1 = FL(1.0) - a1 * a1 * win_fact;
              a1 = a1 * a1 / (MYFLT) d;
              *ar += (MYFLT) x * a1 * ftable[ndx_i];
              d++; v += c * x; x += v;
              if (++ndx_i >= flen) ndx_i = (wrap_ndx ? ndx_i - flen : flen);
            } while (--i);
          }
          else {                /* ---- warp disabled ---- */
            /* avoid division by zero */
            if (ndx < 0.00001) {
              ndx_i += (long) (wsized2 - 1);    /* no need to check here */
              *ar = ftable[ndx_i];
            }
            else if (ndx > 0.99999) {
              ndx_i += (long) wsized2;          /* does need range checking */
              if (ndx_i >= flen) ndx_i = (wrap_ndx ? ndx_i - flen : flen);
              *ar = ftable[ndx_i];
            }
            else {
              /* samples -(window size / 2 - 1) to 0 */
              i = wsized2 >> 1;
              do {
                a1 = (MYFLT) d; a1 = FL(1.0) - a1 * a1 * win_fact;
                *ar += ftable[(ndx_i < 0L ? (wrap_ndx ? ndx_i + flen : 0L)
                                            : ndx_i)] * a1 * a1 / (MYFLT) d;
                d++; ndx_i++;
                a1 = (MYFLT) d; a1 = FL(1.0) - a1 * a1 * win_fact;
                *ar -= ftable[(ndx_i < 0L ? (wrap_ndx ? ndx_i + flen : 0L)
                                            : ndx_i)] * a1 * a1 / (MYFLT) d;
                d++; ndx_i++;
              } while (--i);
              /* samples 1 to (window size / 2) */
              i = wsized2 >> 1;
              do {
                a1 = (MYFLT) d; a1 = FL(1.0) - a1 * a1 * win_fact;
                *ar += a1 * a1 * ftable[ndx_i] / (MYFLT) d;
                d++;
                if (++ndx_i >= flen) ndx_i = (wrap_ndx ? ndx_i - flen : flen);
                a1 = (MYFLT) d; a1 = FL(1.0) - a1 * a1 * win_fact;
                *ar -= a1 * a1 * ftable[ndx_i] / (MYFLT) d;
                d++;
                if (++ndx_i >= flen) ndx_i = (wrap_ndx ? ndx_i - flen : flen);
              } while (--i);
              *ar *= (MYFLT) (sin (PI * ndx) / PI);
            }
          }
          break;
      }
      ar++;             /* next sample */
    }
}

