/* File name: TFMark2.c

   Test studies for the transfer function measurment program.
   
   12Feb2004 .. initial version (TFMark1) .. K.M
   29Feb2004 .. modified for lab 6 .. KM
   
*/   

#include <stdio.h>
#include <math.h>
#include "../support/McBSP_452.h"

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

#define FTV_CNT     33
#define FOREVER     1
#define FS          48000

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


unsigned long ComputeFTV(unsigned long, unsigned long);


// interrupt support definitions

extern unsigned long ftv;
extern volatile int AIC_LeftIn, AIC_RightIn, AIC_LeftOut, AIC_RightOut;
extern volatile int Icos, Qsin, AIC_flag;


unsigned long fs = FS;

double pi=3.14159265;
unsigned long ac;
unsigned Nint = 1*FS/10; // integrate for 0.1 seconds (4800 samples)
unsigned Nstart = 250; // dependent on filter size!!
long xr32, yr32, xd32, yd32;

unsigned int test = 2;  // select which test to execute
int shift;


void main(void)
{
	int idx, sample, sround;
    unsigned long f;
    long round;
    double xrflt, yrflt, rrflt, dang;
    double xdflt, ydflt, rdflt, rang, sf;
    FILE *out;
    
    printf("enter test: ");
    scanf("%d", &test);
    printf("\n");
    
    /*****************************************************************************/
    
    if (test == 1) {
    
        // let the DDS run on right output and echo right input on left out
        
        setup_codec();
        startup();
        
        ftv = ComputeFTV(1000, fs);
        
        _enable_interrupts();
        
        while (FOREVER) {
        	AIC_flag = 0;
        	while (AIC_flag == 0);
        	AIC_LeftOut = AIC_RightIn;
        }
    }
    
    
    if (test == 2) {
    
        // Loop echoing cosine on right and sine on left.
    
        setup_codec();
        startup();
        
        ftv = ComputeFTV(1000, fs);
        
        _enable_interrupts();
    
        while (FOREVER) {
            AIC_flag = 0;
            while (AIC_flag == 0);
            AIC_LeftOut = Qsin;
        }
    }
    
    
    if (test == 3) {
    
    	
        // For analog testing of filter
        
    	// AIC23 left channel A/D input to filter
    	// filter output to ACI23 left channel D/A
    	
    	FilterSetup(); 	// set up filter 
    	
    	setup_codec();
        startup();
        
        ftv = ComputeFTV(1000, fs);
        
        _enable_interrupts();
    
        while (FOREVER) {
            AIC_flag = 0;
            while (AIC_flag == 0);
            AIC_LeftOut = filter(AIC_LeftIn);
        }
    }
 
   
    /********************************************************************************/
    
    else {
    
    // now for a complete spectrum analyzer 
    
    // Measures from 20 Hz to 6000 Hz in 20 Hz steps
    // Integration time is determined by Nint and FS.
    
    // Floating point R calculations to be eliminated in later stand alone version.
    
    	FilterSetup(); // set up filter to use
    	
    	printf("shift samples right by: ");
    	scanf("%d", &shift);
    	printf("\n");
    	sround =0; if (shift>0) sround=(1<<(shift-1)); // for rounding
    
        setup_codec();   // initialize the AIC23 and McBSP
        startup();       // initialize the interrupt support
  
        sf = 2.0*8192.0/((float)Nint*32768.0*32768.0);
        
        out = fopen("tfmeas", "w");              // open output file, name: tfmeas
        if (out == NULL) {
            printf("can't open output file\n");
            exit (1);
        }
        
        for (f = 10; f <= 6000; f+=10) { 
            ftv = ComputeFTV(f,fs);
            xr32 = yr32 = 0L;   // initialize integrators
            xd32 = xd32 = 0L;
            round = (1L<<12);   // used for rounding shifted values
            
            _enable_interrupts();  // only allow ints when needed
            
            for (idx = 0; idx < Nint+Nstart; idx++) {
                
                AIC_flag = 0;
                while (AIC_flag == 0);
                
                sample =  ((AIC_RightIn+sround)>>shift);
                AIC_LeftOut = filter(sample)<<shift;  // filter value and unscale
            
                if (idx < Nstart) continue;    // wait until filter memory is filled
            
                xr32 += ((_lsmpy(Icos, AIC_RightIn)+round) >> 13);
                yr32 -= ((_lsmpy(Qsin, AIC_RightIn)+round) >> 13);
                xd32 += ((_lsmpy(Icos, AIC_LeftIn)+round) >> 13);
                yd32 -= ((_lsmpy(Qsin, AIC_LeftIn)+round) >> 13);
       
            }
            _disable_interrupts();  // seems to make system more stable this way
            
            xrflt = xr32; yrflt = yr32;
            xrflt *= sf; yrflt *= sf;
            rrflt = sqrt(xrflt*xrflt+yrflt*yrflt); // later make fixed point!!!
            xdflt = xd32; ydflt = yd32;
            xdflt *= sf; ydflt *= sf;
            rdflt = sqrt(xdflt*xdflt+ydflt*ydflt); // later make fixed point!!!
            
            // Need to make x and y values 16 bit for myatan2 
            
            while ((labs(xr32) > 32767)|(labs(yr32) > 32767)) {
                xr32 >>= 1; yr32 >>= 1;
            }
            while ((labs(xd32) > 32767)|(labs(yd32) > 32767)) {
                xd32 >>= 1; yd32 >>= 1;
            }
        
            rang = myatan2((int)yr32, (int)xr32)/32768.0; // make fp for common usage testing
            dang = myatan2((int)yd32, (int)xd32)/32768.0; // make fp for common usage testing           
            
            
            /* Data order per line is:
            
            	frequency in Hz
            	r value measured at input to filter
            	r value measured at D/A output from filter
            	angle measured at input to filter (range -1 to 1 half cycles)
            	angle measured at D/A output from filter (range -1 to 1 half cycles)
            	
			*/            	
            
            fprintf(out, " %6lu  %8.6f  %8.6f  %8.6f  %8.6f\n",
                f, rrflt, rdflt, rang, dang);  // used for file output only
        
        }
        fclose(out);
    }
}

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

// Function to compute 32 bit unsigned FTV value give f and fs

unsigned long ComputeFTV(unsigned long f, unsigned long fs)
{
    unsigned idx;
    unsigned long ftv;
    
    ftv = 0;
    
    for (idx = 0; idx < FTV_CNT; idx++) {
        if (f >= fs) {
            ftv = (ftv<<1)+1;
            f -= fs;
        }
        else ftv <<= 1;
        f <<= 1;
    }
    if (f >= fs) ftv += 1;
    
    return (ftv);
}