/*
	dspfft
	
				hacked together by Avery Wang -- April 10, 1992
				Institut fuer Neuroinformatik
				Ruhr-Uni-Bochum

	This code was developed in a manner similar to the way wasp larvae
	feed on their paralyzed prey--eating it from the inside out and
	converting its biomass into its own--similar to what happens to
	the astronauts on "Alien".  This used to be the dsp_dma_stream 
	example given in /NextDeveloper/Examples/DSP


 */

#import <sound/sound.h>
#import <sound/sounddriver.h>
#import <soundkit/soundkit.h>
#import <mach.h>
#import <stdlib.h>
#import <stdio.h>
#import "sd_error.h"	/* should be <sound/snddriver_error.h> */
#import <math.h>
#import <mach_error.h>
#import "dspfft.h"

/* The following must agree with dsp_dma_stream.asm */
#define PI 3.14159265358979
#define ABORT_HOST_COMMAND (0x2E>>1)

#define READ_TAG 1
#define WRITE_TAG 2
#define FLUSH_TAG 3

static port_t cmd_port;
static int done = 0;
static short *read_data;
static int bytes_read;

static port_t reply_port, write_port;
static int sd_err;

static void write_completed(void *arg, int tag)
    /* Called when the input to the DSP has been written. */
{}


static void read_completed(void *arg, int tag, void *p, int nbytes)
// This gets called when the entire result array has been read from the DSP.
{
    if (tag == READ_TAG) {
	read_data = (short *)p;
	bytes_read = nbytes;
	done++;
    }
}

/* 
input is a pointer to blocks of either dspfft_size or 2*dspfft_size float 
data points. The values MUST be normalized to between -1 and 1.
output is a pointer to a block of float memory which is of size (2*dspfft_size*number_of_blocks).
	The data format will be complex, so the output size will be in 2*dspfft_size-float blocks.
complex is a flag:  1=complex values, 0=real values only.  This decides whether
	the block length is dspfft_size (real) or 2*dspfft_size (complex).
number_of_blocks is the number of blocks to transform.  This allows you to
	do several transforms in a row and save the setup overhead.
*/

void dspfft (float *input, float *output, int complex, int number_of_blocks, int dspfft_size)
{
	static int last_xfrm_size=0;
    int i,j,k, s_err, protocol;
	float	*temp_float;
	short	*temp_short;
    kern_return_t k_err;
    port_t dev_port=0, owner_port=0, read_port;
    static Sound	*soundCore=NULL;
    snddriver_handlers_t handlers = { 0, 0, 
    		0, write_completed, 0, 0, 0, 0, read_completed};
    msg_header_t *reply_msg;
    int low_water = 48*1024;
    int high_water = 64*1024;
    int read_width = 2;		/* bytes per sample */
    int read_buf_size;
    short *write_data;
    int write_count;
    int read_count;
    int write_width = 2;	/* bytes per sample */
    int write_buf_size;
	if (dspfft_size<512){
		if (EXPANSION_MEMORY){
			read_buf_size=write_buf_size=4096;
		}
		else
			read_buf_size=write_buf_size=1024;
	}
	else {
		read_buf_size=write_buf_size=2*dspfft_size;
	}
		
	if (last_xfrm_size!=dspfft_size){
		char sndname[20];
		sprintf(sndname,"dspfft%d.snd",dspfft_size);
		soundCore=[Sound findSoundFor:sndname];
		if (soundCore==NULL){// Didn't find it
			fprintf(stderr,"Couldn't find DSP core file %s.  Aborting.\n",sndname);
			exit(1);
		}
		last_xfrm_size=dspfft_size;
	}
    //
    // initialize input buffer with some arbitrary but known data
    //
    write_count = (2*dspfft_size*number_of_blocks/write_buf_size+2)*write_buf_size;
    read_count =  2*dspfft_size*number_of_blocks;

    /* vm_allocate() always grabs whole pages.  We need the alignment. */
    vm_allocate(task_self(),(vm_address_t *)(&write_data),
		write_count*write_width,TRUE);

	if (complex){
		k=2*dspfft_size;
	}
	else {
		k=dspfft_size;
	}
	temp_float=input;
	for (i=0;i<number_of_blocks;i++){
		temp_short=write_data+2*dspfft_size*i;
		for (j=0; j<k; j++){
			*temp_short++ = 32767.0*(*temp_float++);
		}
	}


    //
    // get the device port for the sound/dsp driver on the local machine
    // and try to become owner of the dsp resource.  Note that it fails
    // if you are logged into a remote machine which is not a "public sound
    // server" (as set in the UNIX section of the Preferences application).
    //
    s_err = SNDAcquire(SND_ACCESS_DSP, 0, 0, 0, NULL_NEGOTIATION_FUN,
			0, &dev_port, &owner_port);
    if (s_err != 0) {
	fprintf(stderr,"*** Could not acquire DSP\n");
	exit(1);
    }

    //
    // get the command port
    //
    sd_err = snddriver_get_dsp_cmd_port(dev_port,owner_port,&cmd_port);
    if (sd_err != 0) {
	sd_error("Cannot acquire command port", sd_err);
	exit(1);
    }

    //
    // set up the reading and writing streams, and set the DSP protocol
    //
    protocol = 0;
    sd_err = snddriver_stream_setup(dev_port, owner_port,
    				 SNDDRIVER_DMA_STREAM_FROM_DSP,
				 read_buf_size, read_width,
				 low_water, high_water,
				 &protocol, &read_port);
    if (sd_err != 0) {
	sd_error("Cannot set up stream from DSP", sd_err);
	exit(1);
    }
    
    sd_err = snddriver_stream_setup(dev_port, owner_port,
    				 SNDDRIVER_DMA_STREAM_TO_DSP,
				 write_buf_size, write_width, 
				 low_water, high_water,
				 &protocol, &write_port);
    if (sd_err != 0) {
	sd_error("Cannot set up stream to DSP", sd_err);
	exit(1);
    }

    sd_err = snddriver_dsp_protocol(dev_port, owner_port, protocol);
    if (sd_err != 0) {
	sd_error("Cannot set up DSP protocol", sd_err);
	exit(1);
    }

    //
    // allocate a port for the replies
    //
    k_err = port_allocate(task_self(),&reply_port);
    if (k_err != KERN_SUCCESS) {
	mach_error("Cannot allocate reply port", k_err);
	exit(1);
    }

    //
    // enqueue region read request, asking only for a completion message
    //
    sd_err = snddriver_stream_start_reading(read_port,0,read_count,READ_TAG,
					 	0,1,0,0,0,0, reply_port);
    if (sd_err != 0) {
	sd_error("Cannot enqueue read request", sd_err);
	exit(1);
    }

    //
    // enqueue the region write request. Request only the termination message..
    //
    sd_err = snddriver_stream_start_writing(write_port,
    					 (void *)write_data,write_count,
					 WRITE_TAG,
					 0,1,
					 0,1,0,0,0,0, reply_port);

    if (sd_err != 0) {
	sd_error("Cannot enqueue write request", sd_err);
	exit(1);
    }


    //
    // boot the dsp
    //
    s_err = SNDBootDSP(dev_port, owner_port, [soundCore soundStruct]);
    if (s_err != SND_ERR_NONE) {
	fprintf(stderr,"Cannot boot dsp : %s\n", SNDSoundError(s_err));
	exit(1);
    }


    //
    // receive reply messages (the completion messages) and dispatch.
    //
    reply_msg = (msg_header_t *)malloc(MSG_SIZE_MAX);
    done = 0;

    while (!done) {
		//
		// get a message from DSP
		//
		reply_msg->msg_size = MSG_SIZE_MAX;
		reply_msg->msg_local_port = reply_port;
		k_err = msg_receive(reply_msg, MSG_OPTION_NONE, 0);
		if (k_err != KERN_SUCCESS) {
			mach_error("Cannot receive message from DSP", k_err);
			exit(1);
		}
		
		//
		// dispatch to the appropriate handler
		//
		sd_err = snddriver_reply_handler(reply_msg,&handlers);
		if (sd_err != 0) {
			sd_error("Cannot parse message from DSP", sd_err);
			exit(1);
		}
    }
	
	//OK--got the data!!!
	temp_float=output;
	temp_short=read_data;
	k=number_of_blocks*2*dspfft_size;
	for (i=0;i<k;i++){
		*temp_float++ = *temp_short++*(1./32767);
	}

    //
    // Data received in the read_handler should be deallocated
    //
    vm_deallocate(task_self(),(pointer_t)read_data,bytes_read);
	SNDRelease(SND_ACCESS_DSP, dev_port, owner_port);
 
}
