                                                        /* ugmoss.c */
#include "cs.h"
#include "ugmoss.h"
#include "aops.h"
#include <math.h>

/******************************************************************************
  all this code was written by william 'pete' moss. <petemoss@petemoss.org>
  no copyright, since it seems silly to copyright algorithms.
  do what you want with the code, and credit me if you get the chance
******************************************************************************/

/* rewritten code for dconv, includes speedup tip from
   Moore: Elements of Computer Music */
void dconvset(DCONV *p)
{
    FUNC *ftp;

    p->len = (int)*p->isize;
    if ((ftp = ftfind(p->ifn)) != NULL) {         /* find table */
      p->ftp = ftp;
      if ((unsigned)ftp->flen < p->len)
        p->len = ftp->flen; /* correct len if flen shorter */
    }
    else {
      initerror(Str(X_1557,"No table for dconv"));
      return;
    }
    if (p->sigbuf.auxp == NULL || p->sigbuf.size < (int)(p->len*sizeof(MYFLT)))
      auxalloc(p->len*sizeof(MYFLT), &p->sigbuf);
    p->curp = (MYFLT *)p->sigbuf.auxp;
}

void dconv(DCONV *p)
{
    long i = 0;
    int nsmps = ksmps;
    long len = p->len;
    MYFLT *ar, *ain, *ftp, *startp, *endp, *curp;
    MYFLT sum;

    ain = p->ain;                               /* read saved values */
    ar = p->ar;
    ftp = p->ftp->ftable;
    startp = (MYFLT *) p->sigbuf.auxp;
    endp = startp + len;
    curp = p->curp;

    do {
      *curp = *ain++;                           /* get next input sample */
      i = 1, sum = *curp++ * *ftp;
      while (curp<endp)
        sum += (*curp++ * *(ftp + i++));        /* start the convolution */
      curp = startp;                            /* correct the ptr */
      while (i<len)
        sum += (*curp++ * *(ftp + i++));        /* finish the convolution */
      if (--curp < startp)
        curp += len;                            /* correct for last curp++ */
      *ar++ = sum;
    } while (--nsmps);

    p->curp = curp;                             /* save state */
}

void and_kk(AOP *p)
{
    long input1 = (long)(*p->a + FL(0.5));
    long input2 = (long)(*p->b + FL(0.5));
    *p->r = (MYFLT)(input1 & input2);
}

void and_aa(AOP *p)
{
    MYFLT *r           = p->r;
    MYFLT *in1         = p->a;
    MYFLT *in2         = p->b;
    unsigned int nsmps = ksmps;
    long input1, input2;

    do {
      input1 = (long)(*in1++ + FL(0.5));
      input2 = (long)(*in2++ + FL(0.5));
      *r++ = (MYFLT)(input1 & input2);
    } while (--nsmps);
}

void and_ak(AOP *p)
{
    MYFLT *r = p->r;
    MYFLT *in1 = p->a;
    unsigned int nsmps = ksmps;
    long input2 = (long)(*p->b + FL(0.5)), input1;

    do {
      input1 = (long)(*in1++ + FL(0.5));
      *r++ = (MYFLT)(input1 & input2);
    } while (--nsmps);
}

void and_ka(AOP *p)
{
    MYFLT *r = p->r;
    MYFLT *in2 = p->b;
    unsigned int nsmps = ksmps;
    long input2, input1 = (long)(*p->a + FL(0.5));

    do {
      input2 = (long)(*in2++ + FL(0.5));
      *r++ = (MYFLT)(input1 & input2);
    } while (--nsmps);
}

void or_kk(AOP *p)
{
    long input1 = (long)(*p->a + FL(0.5));
    long input2 = (long)(*p->b + FL(0.5));
    *p->r = (MYFLT)(input1 | input2);
}

void or_aa(AOP *p)
{
    MYFLT *r = p->r;
    MYFLT *in1 = p->a;
    MYFLT *in2 = p->b;
    unsigned int nsmps = ksmps;
    long input2, input1;

    do {
      input1 = (long)(*in1++ + FL(0.5));
      input2 = (long)(*in2++ + FL(0.5));
      *r++ = (MYFLT)(input1 | input2);
    } while (--nsmps);
}

void or_ak(AOP *p)
{
    MYFLT *r = p->r;
    MYFLT *in1 = p->a;
    unsigned int nsmps = ksmps;
    long input2 = (long)(*p->b + FL(0.5)), input1;

    do {
      input1 = (long)(*in1++ + FL(0.5));
      *r++ = (MYFLT)(input1 | input2);
    } while (--nsmps);
}

void or_ka(AOP *p)
{
    MYFLT *r = p->r;
    MYFLT *in2 = p->b;
    unsigned int nsmps = ksmps;
    long input2, input1 = (long)(*p->a + FL(0.5));

    do {
      input2 = (long)(*in2++ + FL(0.5));
      *r++ = (MYFLT)(input1 | input2);
    } while (--nsmps);
}

void xor_kk(AOP *p)
{
    long input1 = (long)(*p->a + FL(0.5));
    long input2 = (long)(*p->b + FL(0.5));
    *p->r = (MYFLT)(input1 ^ input2);
}

void xor_aa(AOP *p)
{
    MYFLT *r = p->r;
    MYFLT *in1 = p->a;
    MYFLT *in2 = p->b;
    unsigned int nsmps = ksmps;
    long input2, input1;

    do {
      input1 = (long)(*in1++ + FL(0.5));
      input2 = (long)(*in2++ + FL(0.5));
      *r++ = (MYFLT)(input1 ^ input2);
    } while (--nsmps);
}

void xor_ak(AOP *p)
{
    MYFLT *r = p->r;
    MYFLT *in1 = p->a;
    unsigned int nsmps = ksmps;
    long input2 = (long)(*p->b + FL(0.5)), input1;

    do {
      input1 = (long)(*in1++ + FL(0.5));
      *r++ = (MYFLT)(input1 ^ input2);
    } while (--nsmps);
}

void xor_ka(AOP *p)
{
    MYFLT *r = p->r;
    MYFLT *in2 = p->b;
    unsigned int nsmps = ksmps;
    long input2, input1 = (long)(*p->a + FL(0.5));

    do {
      input2 = (long)(*in2++ + FL(0.5));
      *r++ = (MYFLT)(input1 ^ input2);
    } while (--nsmps);
}

void not_k(AOP *p)              /* Added for completeness by JPff */
{
    long input1 = (long)(*p->a + FL(0.5));
    *p->r = (MYFLT)(~input1);
}

void not_a(AOP *p)
{
    MYFLT *r = p->r;
    MYFLT *in1 = p->a;
    unsigned int nsmps = ksmps;
    long input1;

    do {
      input1 = (long)(*in1++ + FL(0.5));
      *r++ = (MYFLT)(~input1);
    } while (--nsmps);
}

/* all the vcomb and valpass stuff adapted from comb() and alpass()
   with additional insight from me (petemoss@petemoss.org)  */

void vcombset(VCOMB *p)
{
    long        lpsiz, nbytes;

    if (*p->insmps != FL(0.0)) {
      if ((lpsiz = (long)(FL(0.5)+*p->imaxlpt)) <= 0) {
        initerror(Str(X_867,"illegal loop time"));
        return;
      }
    }
    else if ((lpsiz = (long)(*p->imaxlpt * esr)) <= 0) {
      initerror(Str(X_867,"illegal loop time"));
      return;
    }
    nbytes = lpsiz * sizeof(MYFLT);
    if (p->auxch.auxp == NULL || nbytes != p->auxch.size) {
      auxalloc((long)nbytes, &p->auxch);
      p->pntr = (MYFLT *) p->auxch.auxp;
      if (p->pntr==NULL) {
        initerror(Str(X_668,"Failed to allocate enough space\n"));
        return;
      }
    }
    else if (!(*p->istor)) {
      long *fp = (long *) p->auxch.auxp;
      p->pntr = (MYFLT *) fp;
      do
        *fp++ = 0;
      while (--lpsiz);
    }
    p->rvt = FL(0.0);
    p->lpt = FL(0.0);
    p->g   = FL(0.0);
    p->lpta = (XINARG3) ? 1 : 0;
    if (*p->insmps == 0) *p->imaxlpt *= esr;
}

void vcomb(VCOMB *p)
{
    int nsmps = ksmps;
    unsigned long xlpt, maxlpt = (unsigned long)*p->imaxlpt;
    MYFLT       *ar, *asig, *rp, *endp, *startp, *wp, *lpt;
    MYFLT       g = p->g;

    if (p->auxch.auxp==NULL) {
      perferror(Str(X_1685,"vcomb: not initialised"));
      return;
    }
    ar = p->ar;
    asig = p->asig;
    endp = (MYFLT *) p->auxch.endp;
    startp = (MYFLT *) p->auxch.auxp;
    wp = p->pntr;
    if (p->lpta) {                               /* if xlpt is a-rate */
      lpt = p->xlpt;
      do {
        xlpt = (unsigned long)((*p->insmps != 0) ? *lpt : *lpt * esr);
        if (xlpt > maxlpt) xlpt = maxlpt;
        if ((rp = wp - xlpt) < startp) rp += maxlpt;
        if ((p->rvt != *p->krvt) || (p->lpt != *lpt)) {
          p->rvt = *p->krvt, p->lpt = *lpt;
          g = p->g = (MYFLT)pow(0.001, (p->lpt / p->rvt));
        }
        lpt++;
        *ar = *rp++;
        *wp++ = (*ar++ * g) + *asig++;
        if (wp >= endp) wp = startp;
        if (rp >= endp) rp = startp;
      } while (--nsmps);
    }
    else {                                       /* if xlpt is k-rate */
      xlpt = (unsigned long)((*p->insmps != 0) ? *p->xlpt : *p->xlpt * esr);
      if (xlpt > maxlpt) xlpt = maxlpt;
      if ((rp = wp - xlpt) < startp) rp += maxlpt;
      if ((p->rvt != *p->krvt) || (p->lpt != *p->xlpt)) {
        p->rvt = *p->krvt, p->lpt = *p->xlpt;
        g = p->g = (MYFLT)pow(0.001, (p->lpt / p->rvt));
      }
      do {
        *ar = *rp++;
        *wp++ = (*ar++ * g) + *asig++;
        if (wp >= endp) wp = startp;
        if (rp >= endp) rp = startp;
      } while (--nsmps);
    }
    p->pntr = wp;
}

void valpass(VCOMB *p)
{
    int nsmps = ksmps;
    unsigned long xlpt, maxlpt = (unsigned long)*p->imaxlpt;
    MYFLT       *ar, *asig, *rp, *startp, *endp, *wp, *lpt;
    MYFLT       y, z, g = p->g;

    if (p->auxch.auxp==NULL) {
      perferror(Str(X_1686,"valpass: not initialised"));
      return;
    }
    ar = p->ar;
    asig = p->asig;
    endp = (MYFLT *) p->auxch.endp;
    startp = (MYFLT *) p->auxch.auxp;
    wp = p->pntr;
    if (p->lpta) {                                      /* if xlpt is a-rate */
      lpt = p->xlpt;
      do {
        xlpt = (unsigned long)((*p->insmps != 0) ? *lpt : *lpt * esr);
        if (xlpt > maxlpt) xlpt = maxlpt;
        if ((rp = wp - xlpt) < startp) rp += maxlpt;
        if ((p->rvt != *p->krvt) || (p->lpt != *lpt)) {
        p->rvt = *p->krvt, p->lpt = *lpt;
        g = p->g = (MYFLT)pow(0.001, (p->lpt / p->rvt));
      }
      lpt++;
      y = *rp++;
      *wp++ = z = y * g + *asig++;
      *ar++ = y - g * z;
      if (wp >= endp) wp = startp;
      if (rp >= endp) rp = startp;
      } while (--nsmps);
    }
    else {                                              /* if xlpt is k-rate */
      xlpt = (unsigned long)((*p->insmps != 0) ? *p->xlpt : *p->xlpt * esr);
      if (xlpt > maxlpt) xlpt = maxlpt;
      if ((rp = wp - xlpt) < startp) rp += maxlpt;
      if ((p->rvt != *p->krvt) || (p->lpt != *p->xlpt)) {
        p->rvt = *p->krvt, p->lpt = *p->xlpt;
        g = p->g = (MYFLT)pow(0.001, (p->lpt / p->rvt));
      }
      do {
        y = *rp++;
        *wp++ = z = y * g + *asig++;
        *ar++ = y - g * z;
        if (wp >= endp) wp = startp;
        if (rp >= endp) rp = startp;
      } while (--nsmps);
    }
    p->pntr = wp;
}

void ftmorfset(FTMORF *p)
{
    FUNC *ftp;
    int j = 0;
    unsigned int len;

    if ((ftp = ftfind(p->iresfn)) != NULL) {    /* make sure resfn exists */
      p->resfn = ftp, len = p->resfn->flen;     /* and set it up */
    }
    else {
      initerror(Str(X_1687,"iresfn for ftmorf does not exist"));
      return;
    }

    if ((ftp = ftfind(p->iftfn)) != NULL) {     /* make sure ftfn exists */
      p->ftfn = ftp;                            /* and set it up */
    }
    else {
      initerror(Str(X_1688,"iftfn for ftmorf doesnt exist"));
      return;
    }

    do {                /* make sure tables in ftfn exist and are right size*/
      if ((ftp = ftfind(p->ftfn->ftable + j)) != NULL) {
        if ((unsigned int)ftp->flen != len) {
          initerror(Str(X_1689,"table in iftfn for ftmorf wrong size"));
          return;
        }
      }
      else {
        initerror(Str(X_1690,"table in iftfn for ftmorf does not exist"));
        return;
      }
    } while (++j < p->ftfn->flen);

    p->len = len;
    p->ftndx = -FL(1.0);
}

void ftmorf(FTMORF *p)
{
    unsigned int j = 0;
    int i;
    MYFLT f;
    FUNC *ftp1, *ftp2;

    if (*p->kftndx >= p->ftfn->flen) *p->kftndx = (MYFLT)(p->ftfn->flen - 1);
    i = (int)*p->kftndx;
    f = *p->kftndx - i;
    if (p->ftndx != *p->kftndx) {
      p->ftndx = *p->kftndx;
      ftp1 = ftfind(p->ftfn->ftable + i++);
      ftp2 = ftfind(p->ftfn->ftable + i--);
      do {
        *(p->resfn->ftable + j) = (*(ftp1->ftable + j) * (1-f)) +
          (*(ftp2->ftable + j) * f);
      } while (++j < p->len);
    }
}



/* end of ugmoss.c */
