#include "cs.h"			/*			UGRW2.C	*/
#include "ugrw2.h"
#include <math.h>

/*
 *	Unit generators by Robin Whittle 	9 September 1995	
 *	UGRW2.H contains data structures.
 *	
 *  	Ugens:		Subroutines:	Data structure:
 *
 *	kport		kporset()	KPORT
 *			kport()		"
 *
 *	This is the same as the k rate port (portamento) ugen in ugens5.c, 
 *	except the half time is a k rate variable, rather than an i rate.
 *
 *  	Ugens:		Subroutines:	Data structure:
 *
 *	ktone		ktonset()	KTONE
 *			ktone()		"
 *	katone		katone()	"
 *
 *	kreson		krsnset()	KRESON
 *			kreson()	"
 *			katone()	"
 *
 *	These are the same as tone, atone, reson and areson filters as per
 *	the Csound manual, except they operate with k rate data rather than
 *	a rate.  
 *
 *	The code for all the above has been adapted from a commented version
 *	of ugens5.c.
 *
 *
 *  	Ugens:		Subroutines:	Data structure:
 *
 *	limit		limit()		LIMIT
 *	ilimit		klimit()
 *
 *	limit sets upper and lower limits on k or a rate variables, where
 *	the limits are set by k rate variables.  ilimit does the same for 
 *	init time variables.
 * 
 */

	
/****************************************************************************
 *
 *  	Filters - syntax
 *
*** kr	kport	ksig, khtime [,isig]
 *
 * ksig 	k rate output.
 *
 * khtime	k rate time for output ot reach halfway to input.
 *
 * isig		i time, initialisation for internal state.
 *
 *
*** kr	ktone	ksig, khp [,istor]
*** kr	katone	ksig, khp [,istor]
 *
 * ksig 	k rate output.
 *
 * khp		k rate frequency at which half power is reached.
 *
 * istor	i time, initialise internal state. Default 0 = clear.
 *
 *
*** kr	kreson	ksig, kcf, kbw [, iscl] [,istor] 
*** kr	kareson	ksig, kcf, kbw [, iscl] [,istor] 
 *
 * ksig 	k rate output.
 *
 * kcf		k rate centre frequency.
 *
 * kbw		k rate bandwidth - between lower and upper half power points.
 *
 * iscl		i time, 0 = no gain change (default or a rate reson, areson
 *			    		    but leads to huge signals.)	
 *			1 = gain at centre = 1 (default in kreson, kareson)
 *			2 = overal RMS gain = 1 (What exactly does this mean?)
 *
 * istor	i time, initialise internal state. Default 0 = clear.
 *
 *
 *>>> 	Add these to the end of opcodlst in entry.c.
 *
 * opcode   dspace	thread	outarg	inargs	 isub	 ksub	 asub	
 *

{ "kport",   S(KPORT),	3,	"k",	"kko",	kporset, kport, NULL	},
{ "ktone",   S(KTONE),	3,	"k",	"kko",	ktonset, ktone, NULL	},
{ "katone",  S(KTONE),	3,	"k",	"kko",	ktonset, katone, NULL	},
{ "kreson",  S(KRESON),	3,	"k",	"kkkpo",krsnset, kreson, NULL	},
{ "kareson", S(KRESON),	3,	"k",	"kkkpo",krsnset, kareson, NULL	},

 *
 */


/****************************************************************************
 *
 *	limit, ilimit - syntax
 *
*** ir	ilimit	isig, ilow, ihigh
 *
*** kr	limit	ksig, klow, khigh
*** ar	limit 	asig, klow, khigh
 *
 *	These set lower and upper limits on the xsig value they process.
 *
 *	If xhigh is lower than xlow, then the output will be the average of 
 *	the two - it will not be affected by xsig.
 *
 *>>> 	Add these to the end of opcodlst in entry.c.
 *
 * opcode   dspace	thread	outarg	inargs	 isub	 ksub	 asub	
 *

{ "ilimit", S(LIMIT),	1,	"i",	"iii",	 klimit, NULL, 	 NULL},
{ "limit",  S(LIMIT),	7,	"s",	"xkk",	 limitset, klimit, limit},

 *
 */




/* 	! ! ! !
 *
 *	See the end of this file for details of what other things to 
 *	add to entry.c and how to tweak the makefile.
 */

/*---------------------------------------------------------------------------*/


					/* kport = portamento
					 *
					 * k rate low pass filter  with half 
					 * time controlled by a k rate value.
					 */
void kporset(KPORT *p)
{
					/* Set internal state to last input
					 * variable, but only if it is not
					 * negative. (Why?)
					 */
	if (*p->isig >= 0)
		p->yt1 = *p->isig;

	p->prvhtim = -100;
}

					/*-----------------------------------*/
					
					/* kport function.
					 */

void kport(KPORT *p)
{
					/* Set up variables local to this 
					 * instance of the port ugen, if
					 * khtim has changed.
					 *
					 * onedkr = one divided by k rate.
					 *
					 * c2 = sqrt 1 / kr * half time 
					 *
					 * c1 = 1 - c2
					 *
					 * Note double precision.
					 */
	if (p->prvhtim != *p->khtim)	
	{
		p->c2 = pow((double).5, (double)onedkr / *p->khtim);
		p->c1 = 1. - p->c2;

		p->prvhtim = *p->khtim;
	}
					
					/* New state =   (c2 * old state)
					 *	       + (c1 * input value)
					 */	      

	*p->kr = p->yt1 = p->c1 * *p->ksig + p->c2 * p->yt1;
}


/*****************************************************************************/

					/* ktone = low pass single pole
					 * 	  filter.
					 *
					 * ktonset function.
					 */
void ktonset(KTONE *p)
{
					/* Initialise internal variables to 
					 * 0 or 1.
					 */ 
	p->c1 = p->prvhp = 0;
	p->c2 = 1;
	if (!(*p->istor))
		p->yt1 = 0;
}
					/*-----------------------------------*/

					/* ktone function
					 */
void ktone(KTONE *p)
{
					/* If the current frequency is 
					 * different from before, then 
					 * calculate new values for 
					 * c1 and c2.
					 */
	if (*p->khp != p->prvhp) {
		float b;
		p->prvhp = *p->khp;
					/* b = cos ( 2 * pi * freq / krate)
					 *	
					 * c2 = b - sqrt ( b squared - 1)
					 *
					 * c1 = 1 - c2
					 *
					 * tpidsr = 2 * pi / a sample rate
					 * so tpidsr * ksmps =
					 *  2 * pi / k rate.	
					 * We need this since we are filtering
					 * at k rate, not a rate.
					 */

		b = 2. - cos((double)(*p->khp * tpidsr * ksmps));
		p->c2 = b - sqrt((double)(b * b - 1.));
		p->c1 = 1. - p->c2;
	}
					/* Filter calculation on each a rate
					 * sample:
					 *
					 * New state =   (c2 * old state)
					 *	       + (c1 * input value)
					 */

	*p->kr = p->yt1 = p->c1 * *p->ksig + p->c2 * p->yt1;
}
	
					/*-----------------------------------*/

					/* katone = high pass single pole
					 * 	    filter
					 *
					 * Uses toneset to set up its 
					 * variables.
					 *
					 * Identical to tone, except for the
					 * output calculation.
					 */
void katone(KTONE *p)
{
	if (*p->khp != p->prvhp) {
		float b;
		p->prvhp = *p->khp;
		b = 2. - cos((double)(*p->khp * tpidsr * ksmps));
		p->c2 = b - sqrt((double)(b * b - 1.));
		p->c1 = 1. - p->c2;
	}
					/* Output = c2 * (prev state + input)
					 *
					 * new state = prev state - input
					 *
					 * So c1 is not used.
					 */

	*p->kr = p->yt1 = p->c2 * (p->yt1 + *p->ksig);
	p->yt1 -= *p->ksig;		/* yt1 contains yt1-xt1 */
}


/*****************************************************************************/

					/* kreson = resonant filter
					 */

void krsnset(KRESON *p)
{
					/* Check scale = 0, 1 or 2.
					 */
register int scale;

        p->scale = scale = *p->iscl;
	if (scale && scale != 1 && scale != 2) {
	        sprintf(errmsg,"Illegal resonk iscl value, %f",*p->iscl);
		initerror(errmsg);
	}
					/* Put dummy values into 
					 * previous centre freq and 
					 * bandwidth.
					 */
	p->prvcf = p->prvbw = -100.;
					/* Set intial state to 0 if istor = 0.
					 */				

	if (!(*p->istor))
		p->yt1 = p->yt2 = 0.;
}

					/*-----------------------------------*/

					/* kreson
					 */	
void kreson(KRESON *p)
{
					/* Local variables for this k sample.
					 *
					 * Integers:
					 *
					 * flag		Set to 1 if either
					 *		centre freq or 
					 *		bandwidth has changed.
					 */
register int	flag = 0;

					/* Floats:
					 *
					 * Temporary values for calculating
					 * filter factors.
					 */
register float	c3p1, c3t4, omc3, c2sqr;

					/* Calculations for centre frequency
					 * if it changes.
					 *
					 * cosf = cos (2pi * freq / krate)
					 *
					 */
	if (*p->kcf != p->prvcf) {
		p->prvcf = *p->kcf;
		p->cosf = cos((double)(*p->kcf * tpidsr * ksmps));
		flag = 1;
	}

					/* Calculations for bandwidth if it
					 * changes.
					 *
					 * c3 = exp (-2pi * bwidth / krate)
					 */
	if (*p->kbw != p->prvbw) {
		p->prvbw = *p->kbw;
		p->c3 = exp((double)(*p->kbw * mtpdsr * ksmps));
		flag = 1;
	}
					/* Final calculations for the factors
					 * for the filter. Each multiplies
					 * something and sums it to be the
					 * output, and the input to the first
					 * delay.
					 *
					 * c1	Gain for input signal. 
					 *
					 * c2  	Gain for output of delay 1.
					 *		
					 * c3	Gain for output of delay 2.
					 */

	if (flag) {
		c3p1 = p->c3 + 1.;
		c3t4 = p->c3 * 4.;
		omc3 = 1 - p->c3;

					/* c2= (c3 * 4 * cosf / (c3 + 1)
					 */	

		p->c2 = c3t4 * p->cosf / c3p1;		/* -B, so + below */
		c2sqr = p->c2 * p->c2;

		if (p->scale == 1)
					/* iscl = 1. Make gain at centre = 1.
					 *
					 * c1= (1 - c3) * 
					 *	sqrt( 1 - (c2 * c2 / (c3 * 4) )
					 */
			p->c1 = omc3 * sqrt((double)1. - c2sqr / c3t4);


		else if (p->scale == 2)
					/* iscl = 2 Higher gain, so "RMS gain"
					 * = 1.
					 *
					 * c1= sqrt((c3 + 1)*(c3 + 1) - cs*c2)
					 *    * (1 - c3) / (c3 + 1)
					 *
					 * (I am not following the maths!)
					 */

			p->c1 = sqrt((double)((c3p1*c3p1-c2sqr) * omc3/c3p1));

					/* iscl = 0. No scaling of the signal.
					 * Input gain c1 = 1.
					 */
		else p->c1 = 1.;
	}

					/* Filter section, see c1, c2, c3 notes
					 * above.  Calculate output and 
					 * the new values for the first and
					 * second delays.
					 */

	*p->kr = p->c1 * *p->ksig + p->c2 * p->yt1 - p->c3 * p->yt2;
	p->yt2 = p->yt1;
	p->yt1 = *p->kr;
}


/*****************************************************************************/

					/* karseson - band reject filter.
					 *
					 * uses krsnset() above.
					 *
					 * Comments not done yet. Modifications
					 * to make it k rate done with great
					 * care!
					 */

void kareson(KRESON *p)
{
register int	flag = 0;
register float	c3p1, c3t4, omc3, c2sqr, D = 2.; /* 1/RMS = root2 (rand) */
                                                 /*      or 1/.5  (sine) */
	if (*p->kcf != p->prvcf) {
		p->prvcf = *p->kcf;
		p->cosf = cos((double)(*p->kcf * tpidsr * ksmps));
		flag = 1;
	}
	if (*p->kbw != p->prvbw) {
		p->prvbw = *p->kbw;
		p->c3 = exp((double)(*p->kbw * mtpdsr * ksmps));
		flag = 1;
	}
	if (flag) {
		c3p1 = p->c3 + 1.;
		c3t4 = p->c3 * 4.;
		omc3 = 1 - p->c3;
		p->c2 = c3t4 * p->cosf / c3p1;
		c2sqr = p->c2 * p->c2;
		if (p->scale == 1)			/* i.e. 1 - A(reson) */
			p->c1 = 1. - omc3 * sqrt((double)1. - c2sqr / c3t4);
		else if (p->scale == 2)                 /* i.e. D - A(reson) */
			p->c1 = D - sqrt((double)((c3p1*c3p1-c2sqr)*omc3/c3p1));
		else p->c1 = 0.;                        /* can't tell        */
	}

	if (p->scale == 1 || p->scale == 0) 
	{
		*p->kr = p->c1 * *p->ksig + p->c2 * p->yt1 - p->c3 * p->yt2;
		p->yt2 = p->yt1;
		p->yt1 = *p->kr - *p->ksig;	/* yt1 contains yt1-xt1 */
	}

	else if (p->scale == 2) 
	{
		*p->kr = p->c1 * *p->ksig + p->c2 * p->yt1 - p->c3 * p->yt2;
		p->yt2 = p->yt1;
		p->yt1 = *p->kr - D * *p->ksig;	/* yt1 contains yt1-D*xt1 */
	}
}


/*****************************************************************************/
/*****************************************************************************/


					/* limit and ilimit
					 */

void limitset(LIMIT *p)
					/* Because we are using Csounds
					 * facility (thread = 7) for 
					 * deciding whether this is a or k 
					 * rate, we must provide an init time
					 * setup function.
					 *
					 * In this case, we have nothing to do.
					 */
{

}

					/* klimit()
					 *
					 * Used for k and i rate variables.
					 */
void klimit(LIMIT *p)

{
					/* Local variables.
					 *
					 * xsig		Local register
					 *		copy of input signal.
					 *
					 * xlow		Local register copies
					 * xhigh	of limits.
					 */
register float	xsig, xlow, xhigh;

					/* Optimise for speed when xsig is
					 * within the limits. 
					 */			

	if (    ( (xsig = *p->xsig) <= *p->xhigh )
	     && ( xsig >= *p->xlow )
           )	
	{
		*p->xdest = xsig;
	}
	
	else
	{
					/* xsig was not within the limits.
					 *
					 * Check to see if xlow > xhigh.
					 * If so, then the result will be
					 * the mid point between them.
					 */ 

		if ( (xlow = *p->xlow) >= (xhigh = *p->xhigh) )
		{
			*p->xdest = (xlow + xhigh) / 2;
		}
		else
		{
			if (xsig > xhigh)
				*p->xdest = xhigh;	
			
			else			
				*p->xdest = xlow;
		}
	}
}

					/*************************************/

					/* alimit()
					 *
					 * Used for a rate variables, with 
					 * k rate limits.
					 */
void limit(LIMIT *p)

{
					/* Local variables.
					 *
					 * adest	Pointer to output.
					 *
					 * asig		Pointer to input.
					 *
					 */

register float 	*adest, *asig;

					/* xlow		Local register copies
					 * xhigh	of limits.
					 *
					 * xaverage	Midpoint between these
					 * 		if xlow > xhigh.
					 *
					 * xsig		The input sample 
					 *		currently being tested.
					 */

register float	xlow, xhigh, xaverage, xsig;

					/* Loopcounter.
					 */
register int loopcount = ksmps;
					/*-----------------------------------*/

					/* Optimise for speed when xsig is
					 * within the limits. 
					 */			

					/* Set up input and output pointers.
					 */ 
	adest = p->xdest;
	asig  = p->xsig;
					/* Test to see if xlow > xhigh.
					 * if so, then write the average to 
					 * the destination - no need to 
					 * look at the input.
					 *
					 * Whilst doing the test, load the
					 * limit local variables.
					 */

	if ( (xlow = *p->xlow) >= (xhigh = *p->xhigh) )
	{
		xaverage = (xlow + xhigh) / 2;

		do *adest++ = xaverage;
		while (--loopcount);
	}
	
	else

	do
	{
					/* Do normal processing, testing
					 * each input value against the 
					 * limits.
					 */

		if (    ( (xsig = *asig++) <= xhigh )
		     && ( xsig >= xlow )
	           )	
		{
					/* xsig was within the limits.
					 */
			*adest++ = xsig;
		}
	
		else
		{
			if (xsig > xhigh)
				*adest++ = xhigh;	
			
			else			
				*adest++ = xlow;
		}
	} while (--loopcount);
}


/****************************************************************************
 *
 * 	What to add to other files to get this going??
 *	----------------------------------------------
 */


/* Add two sets of entries to those at the end of opcodlst in entry.c.
 *
 * These are sets of lines in curly braces and are at the start of this
 * file and are marked *>>>
 *
 * In addition, add the following function prototypes to into entry.c:
 * 
 * 	#include "ugrw2.h"
 *
 *	void 	kporset(void*), kport(void*);	
 *	void	ktonset(void*), ktone(void*), katone(void*);
 *	void	krsnset(void*), kreson(void*), kareson(void*);
 *
 *	void	limitset(void*), klimit(void*), limit(void*);
 */


/* Add some stuff to the makefile to make this module UGRW1.C and .H part of
 * the compilation process.
 *
 * Add ugrw2.o to variable OBJS.
 *
 * Add ugrw2.h as a dependency for entry.o.
 *
 * Create a new dependency, ugrw2.o:  ugrw2.h
 *
 * . . . . well that is what Appendix 8 of the CSound manual told me to do.
 *
 * Now here is what I actually did:
 *
 * Add ugrw2.o to the ends of the lists called COBJS and LCOBJS.
 *	(I have not looked into the details of makefiles, but I guess these
 *	 are for the compiler and linker.)
 *
 * In the list of things starting with "entry.o:", add ugrw2.h.  The order 
 * looks like it is significant, so I put my entries after soundio.h (the last one
 * that looked like an ordinary piece of code.)
 *
 * Create a new dependency to fire up the compiler to make a new ugrw2.o 
 * object file.  This is copied from similar lines for other modules.
 *
 * I put it after the cscormai.o: dependency.  
 *
 *	ugrw2.o:      ugrw2.h sysdep.h prototypes.h cs.h ugrw2.c
 *			$(CC) $(CFLAGS) -c $*.c
 */

/*
 * (Some notes specific to DJGPP compiler for MSDOS)
 *
 * The second line above creates the command line for compiling this module.
 * "$(CC) is converted to CPP which is the DJGPP compiler. "$(CFLAGS) is 
 * converted to other things. "-c" goes straight onto the command line.
 * "$*.c" is trasformed into ugrw1.c.
 *
 * To get a file of the problems that the compiler reports:
 * 
 * 1: Add 2r1 to the GO32 environment variable.
 *
 * 2: Add "> bugs" to the end of the second line so it becomes:
 *
 *		$(CC) $(CFLAGS) -c $*.c > bugs
 */