/******** Osc2PWM *******************
* by Sean Montgomery 2008/05
* Reads data from input port and performs an FFT 
* to separate changes in spectral power from 3 specified freq bands
* onto 3 RGB LEDs where increases in power from baseline
* turn the LEDs RED and decreases turn the LEDs blue
************************************/

/**** fft plan *****
* general thoughts
256Hz sampling
128 samp window
3/4 window overlap -> fft (1/8 sec)
power smoothed over 2 points (1/4 sec)

* ititialization
fractional* twiddleFactors[64] = twiTwidFactorInit (in x-data or program mem)
initialize hanningWin[128] (in prog mem? maybe faster in y-data)
declare fractional* tempVec[32] (maybe faster in y-data)
declare fractional* dataVec[128] (in x-data)
declare fractcomplex* sigCmplx[128] in y-data
char spectralReadyBit = 1;

* data processing
fill tempVec[32] with value/2 or fill dataVec[128] with value/2
if full
	memmove(dataVec)
	memcopy(tempVec,dataVec)
	if spectralReadyBit
		spectralReadyBit = 0;
		call fftFunc
			vectorWindow dataVec[128] with hanning to sigCmpx[0].real - 538 cycles (18us)
			point sigComplex.real to sigReal & init sigComplex.imag  - ? cycles 
			FFTComplexIP - 9,383 cycles max (312us)
			BitReverseComplex (in place) - 725 cycles (25us)
			SquareMagnitudeCplx (in place) - 404 cycles (14us)
			get power in freq 
		end
		spectralReadyBit = 1;
		average over time
	end
end
data2hue
hue2rgb
*******************/
/* 
* The input and output sequences to the FFT family of transforms must be
* allocated in Y-Data memory.
*/

#include <p30f3013.h>
#include <timer.h>
#include <adc12.h>
#include <dsp.h>
#include "rgb2pwm.h"
#include "clip.h"
#include "SetOutputs16bit.h"

typedef struct {
	int freqRange[2]; 
	int binRange[2]; // bins of fft output corresponding to freqRange
	float power;
	float meanPower;
	float sdPower;
	RGB rgb;
} brainwave_struct;

typedef struct {
	unsigned char * r;
	unsigned char * g;
	unsigned char * b;
} RGBpointers;

/* Constant Definitions */
#define NUM_BRAINWAVES		3
#define FFT_BLOCK_LENGTH	128     /* = Number of frequency points in the FFT */
#define TEMP_BUFF_LENGTH	32		// size of short buffer gathering data
#define LOG2_BLOCK_LENGTH 	7	/* = Number of "Butterfly" Stages in FFT processing */
#define SAMPLING_RATE		256	/* = Rate at which input signal was sampled */
                                        /* SAMPLING_RATE is used to calculate the frequency*/
                                        /* of the largest element in the FFT output vector*/
//#define FFTTWIDCOEFFS_IN_PROGMEM 	/*<---Comment out this line of the code if twiddle factors (coefficients) */
                                	/*reside in data memory (RAM) as opposed to Program Memory */
                                	/*Then remove the call to "TwidFactorInit()" and add the twiddle factor*/
                                	/*coefficient file into your Project. An example file for a 256-pt FFT*/
                                	/*is provided in this Code example */

void init(void);
void InitRGBLEDs(void);
void InitBrainWaves(void);
void GetData(void);
void ProcessData(void);
void __attribute__((__interrupt__)) _ISR _T1Interrupt();
void __attribute__((__interrupt__)) _ISR _T3Interrupt();


/* Global Definitions */
#ifndef FFTTWIDCOEFFS_IN_PROGMEM
fractcomplex twiddleFactors[FFT_BLOCK_LENGTH/2] 	/* Declare Twiddle Factor array in X-space*/
__attribute__ ((section (".xbss, bss, xmemory"), aligned (FFT_BLOCK_LENGTH*2)));
#else
extern const fractcomplex twiddleFactors[FFT_BLOCK_LENGTH/2]	/* Twiddle Factor array in Program memory */
__attribute__ ((space(auto_psv), aligned (FFT_BLOCK_LENGTH*2)));
#endif

fractcomplex sigCmpx[FFT_BLOCK_LENGTH] 		/* Typically, the input signal to an FFT  */
__attribute__ ((section (".ybss,bss,ymemory"), 	/* routine is a complex array containing samples */
aligned (FFT_BLOCK_LENGTH * 2 *2)));      		/* of an input signal. For this example, */
							/* we will provide the input signal in an */
							/* array declared in Y-data space. */

fractional hanningWin[FFT_BLOCK_LENGTH]
__attribute__ ((section (".xbss, bss, xmemory"), 
aligned (FFT_BLOCK_LENGTH * 2)));  

fractional signalBuffer[FFT_BLOCK_LENGTH]
__attribute__ ((section (".xbss, bss, xmemory"), 
aligned (FFT_BLOCK_LENGTH * 2)));      		

static fractional tempBuffer[TEMP_BUFF_LENGTH];
static int tempBuffPos = 0;
static int	peakFrequencyBin = 0;
static unsigned long peakFrequency = 0;
static char spectralReadyBit = 1;
static unsigned long misses = 0;

static brainwave_struct brainWaves[NUM_BRAINWAVES];
static fractional smoothTemp = 0.0; // used for pre-filtering to avoid aliasing in FFT
static fractional preFiltPeriod = 1.0; // used to smooth out high freq signal
static fractional maxPreFilt = 2.0; 
static float smoothPeriod = 1.0; // used to smooth power estimates
static unsigned long maxSmoothPeriod = 2 * SAMPLING_RATE / TEMP_BUFF_LENGTH; // specify in seconds
static float meanPeriod = 1.0; // period of mean and stdev calculations
static unsigned long maxMeanPeriod = (unsigned long) 1200 * SAMPLING_RATE / TEMP_BUFF_LENGTH; // specify in seconds

static int testingCounter = 0;

//--- Set up LED output mapping ---//
#define LEDON 		0
#define NUMLEDPINS	9
volatile unsigned int * outPorts[NUMLEDPINS] = {
					&LATB, // R
					&LATB, // G
					&LATB, // B

					&LATB, // R
					&LATB, // G
					&LATB, // B

					&LATC, // R
					&LATC, // G
					&LATD, // B
					};
unsigned int  outBits[NUMLEDPINS] = {
					0b0000000001000000, // R
					0b0000000010000000, // G
					0b0000000100000000, // B

					0b0000000000001000, // R
					0b0000000000010000, // G
					0b0000000000100000, // B

					0b0010000000000000, // R
					0b0100000000000000, // G
					0b0000001000000000, // B
					};
unsigned char outVals[NUMLEDPINS] = {
					LEDON,
					LEDON,
					LEDON,

					LEDON,
					LEDON,
					LEDON,

					LEDON,
					LEDON,
					LEDON,
					};

#define MAXPWM  32 // resolution of PWM, 32=5bit
static RGBPWM pwmVals[NUM_BRAINWAVES]; // structure for handling RGB PWM
static RGBpointers outLEDs[NUM_BRAINWAVES]; // for mapping outVals onto RGB LEDs


void InitRGBLEDs(void) {
	unsigned char j;
	for (j=0;j<NUM_BRAINWAVES;j++) {
		// initialize RGBPWM structure for handling RGB PWM
		pwmVals[j]  = initRGBPWM( pwmVals[j], MAXPWM ) ;

		// map output vector onto rgb arrays
		outLEDs[j].r = &outVals[j*3];
		outLEDs[j].g = &outVals[j*3+1];
		outLEDs[j].b = &outVals[j*3+2];
	}
}

void InitBrainWaves(void) {
	unsigned int j, k;
	brainWaves[0].freqRange[0] = 8; // best if each value is a factor of (SAMPLING RATE/FFT_BLOCK_LENGTH);
	brainWaves[0].freqRange[1] = 12; // best if each value is a factor of (SAMPLING RATE/FFT_BLOCK_LENGTH);
	brainWaves[1].freqRange[0] = 16; // best if each value is a factor of (SAMPLING RATE/FFT_BLOCK_LENGTH);
	brainWaves[1].freqRange[1] = 30; // best if each value is a factor of (SAMPLING RATE/FFT_BLOCK_LENGTH);
	brainWaves[2].freqRange[0] = 30; // best if each value is a factor of (SAMPLING RATE/FFT_BLOCK_LENGTH);
	brainWaves[2].freqRange[1] = 50; // best if each value is a factor of (SAMPLING RATE/FFT_BLOCK_LENGTH);

	for (j=0;j<NUM_BRAINWAVES;j++) {		
		for (k=0;k<2;k++){
			brainWaves[j].binRange[k] = ( brainWaves[j].freqRange[k] * (FFT_BLOCK_LENGTH/2) ) / SAMPLING_RATE;
		}
		brainWaves[j].power = 0.0;
		brainWaves[j].meanPower = 0.0;
		brainWaves[j].sdPower = 0.0;
	}
}



int main(void) {
	unsigned char hue = 0;
	int j;
	float sdFactor = 2.0;
	float temp;

	InitRGBLEDs();
	InitBrainWaves();
	init();

	while (1) {
		for (j=0;j<NUM_BRAINWAVES;j++) {
			temp = brainWaves[j].power - brainWaves[j].meanPower;
			temp = temp / brainWaves[j].sdPower;
			temp = temp / sdFactor;
			hue = (short) clip( 85 + ((int) (85.0 * temp)),0,170);
			brainWaves[j].rgb = hue2rgb(hue);
			brainWaves[j].rgb.r = brainWaves[j].rgb.r/(256/MAXPWM);
			brainWaves[j].rgb.g = brainWaves[j].rgb.g/(256/MAXPWM);
			brainWaves[j].rgb.b = brainWaves[j].rgb.b/(256/MAXPWM);
		}
	}
	return 1;
}

void GetData(void)
{
	fractional temp;

	while (BusyADC12());
	temp = ReadADC12(0) * 0.5; // mult by 0.5 to meet fft requirment

	// pre-filter/smooth incoming data to avoid aliasing in FFT
	// would be better to use an FIR filter in to dspic libraries
	smoothTemp = smoothTemp*((preFiltPeriod-1.0)/preFiltPeriod);
	smoothTemp = smoothTemp + (temp/preFiltPeriod);
	if (preFiltPeriod < maxPreFilt) { preFiltPeriod = preFiltPeriod + 1.0; }

	tempBuffer[tempBuffPos] = smoothTemp; 
	ADCON1bits.SAMP=1;
	tempBuffPos++;

	// Test time interval of sampling interrupt (128Hz)
	if (testingCounter == 1) { 
		// PORTDbits.RD8=LEDON;
		PORTFbits.RF6=LEDON;
		testingCounter = 0;
	} else {
		// PORTDbits.RD8=!LEDON;
		PORTFbits.RF6=!LEDON;
		testingCounter++;
	}
	
	
	if (tempBuffPos == TEMP_BUFF_LENGTH) {   // tempbuffer is full
		VectorCopy(FFT_BLOCK_LENGTH - TEMP_BUFF_LENGTH, 
			&signalBuffer[TEMP_BUFF_LENGTH],&signalBuffer[0]);
		VectorCopy(TEMP_BUFF_LENGTH,&signalBuffer[0],&tempBuffer[0]);
		tempBuffPos = 0;
		if (spectralReadyBit) {
			PORTDbits.RD8=!LEDON; 
			spectralReadyBit = 0;
			ProcessData();
			spectralReadyBit = 1;
		} else { 
			PORTDbits.RD8=LEDON; // Display misses
			misses++;
		}
	}
}

void ProcessData(void) {
	int i, j;
	float tempPower;
	fractional *p_real = &sigCmpx[0].real ;
	fractcomplex *p_cmpx = &sigCmpx[0] ;
	
	// Should run a low-pass filter here to reduce aliasing in FFT

	VectorWindow(FFT_BLOCK_LENGTH, &sigCmpx[0].real, // taper signal
		&signalBuffer[0], &hanningWin[0]); 
	//VectorCopy(FFT_BLOCK_LENGTH,&sigCmpx[0].real,&signalBuffer[0]);

	p_real = &sigCmpx[(FFT_BLOCK_LENGTH/2)-1].real ;	/* Set up pointers to convert real array */
	p_cmpx = &sigCmpx[FFT_BLOCK_LENGTH-1] ; /* to a complex array. The input array initially has all */
						/* the real input samples followed by a series of zeros */

	for ( i = FFT_BLOCK_LENGTH; i > 0; i-- ) /* Convert the Real input sample array */
	{					/* to a Complex input sample array  */
		(*p_cmpx).real = (*p_real--);	/* We will simpy zero out the imaginary  */
		(*p_cmpx--).imag = 0x0000;	/* part of each data sample */
	}
	
	//VectorZeroPad(0, FFT_BLOCK_LENGTH, &sigCmpx[0].imag, &sigCmpx[0].imag);

	/* Perform FFT operation */
#ifndef FFTTWIDCOEFFS_IN_PROGMEM
	FFTComplexIP (LOG2_BLOCK_LENGTH, &sigCmpx[0], &twiddleFactors[0], COEFFS_IN_DATA);
#else
	FFTComplexIP (LOG2_BLOCK_LENGTH, &sigCmpx[0], (fractcomplex *) __builtin_psvoffset(&twiddleFactors[0]), (int) __builtin_psvpage(&twiddleFactors[0]));
#endif
	
	/* Store output samples in bit-reversed order of their addresses */
	BitReverseComplex (LOG2_BLOCK_LENGTH, &sigCmpx[0]);

	/* Compute the square magnitude of the complex FFT output array so we have a Real output vetor */
	SquareMagnitudeCplx(FFT_BLOCK_LENGTH, &sigCmpx[0], &sigCmpx[0].real);


	// Find the frequency Bin ( = index into the SigCmpx[] array) that has the largest energy
	// i.e., the largest spectral component 
	VectorMax((FFT_BLOCK_LENGTH/2)-1, &sigCmpx[1].real, &peakFrequencyBin);
    peakFrequencyBin = peakFrequencyBin + 1;

	// Compute the frequency (in Hz) of the largest spectral component 
	peakFrequency = (peakFrequencyBin)*(SAMPLING_RATE/FFT_BLOCK_LENGTH);


	// For each frequency band:
	//	Average over specified freq band
	//	Smooth power over smoothPeriod
	//	Calculate running meanPower
	//	Calculate running sdPower
	// 	Increment smoothPeriod and meanPeriod
	for (j=0;j<NUM_BRAINWAVES;j++) {
		tempPower = 0.0;
		for (i=brainWaves[j].binRange[0];i<=brainWaves[j].binRange[1];i++) {
			tempPower = tempPower + 
				( Fract2Float(sigCmpx[i].real) / 
				((float) (1 + brainWaves[j].binRange[1] - brainWaves[j].binRange[0])) );
		}
		//	Smooth power over smoothPeriod
		brainWaves[j].power = brainWaves[j].power*((smoothPeriod-1.0)/smoothPeriod);
 		brainWaves[j].power = brainWaves[j].power + (tempPower/smoothPeriod);

		//	Calculate running meanPower
		brainWaves[j].meanPower = brainWaves[j].meanPower*((meanPeriod-1.0)/meanPeriod);
 		brainWaves[j].meanPower = brainWaves[j].meanPower + (brainWaves[j].power/meanPeriod);

		//	Calculate running sdPower
		brainWaves[j].sdPower = brainWaves[j].sdPower*((meanPeriod-1.0)/meanPeriod);
 		brainWaves[j].sdPower = brainWaves[j].sdPower 
			+ (fabs(brainWaves[j].meanPower - brainWaves[j].power)/meanPeriod);	
	}
	// 	Increment smoothPeriod and meanPeriod
	if (smoothPeriod < ((float) maxSmoothPeriod)) { smoothPeriod = smoothPeriod + 1.0; }
	if (meanPeriod < ((float) maxMeanPeriod)) { meanPeriod = meanPeriod + 1.0; }
}
	
void __attribute__((interrupt,no_auto_psv)) _ISR _T1Interrupt()          //this handles timer1 Interrupts
{
	unsigned char j;
	/***** Timer 0 Code *****/
	if((IEC0bits.T1IE)&&(IFS0bits.T1IF)){
		// TODO - add code to handle this event...
		IFS0bits.T1IF=0;	// clear event flag
		WriteTimer1(0);

		for (j=0;j<NUM_BRAINWAVES;j++) {
			pwmVals[j] = rgb2pwm( brainWaves[j].rgb, pwmVals[j] );
			*outLEDs[j].r = ( pwmVals[j].rOn == LEDON );
			*outLEDs[j].g = ( pwmVals[j].gOn == LEDON );
			*outLEDs[j].b = ( pwmVals[j].bOn == LEDON );
		}
		SetOutputs16bit(NUMLEDPINS,outVals,outPorts,outBits);
	}
}

void __attribute__((interrupt,no_auto_psv)) _ISR _T3Interrupt()          //this handles timer1 Interrupts
{
	if((IEC0bits.T3IE)&&(IFS0bits.T3IF)){
		// TODO - add code to handle this event...
		IFS0bits.T3IF=0;	// clear event flag
		WriteTimer3(0);
		GetData();
	}
}

void init(void) {

	unsigned int
        ui16ADCON1,                     // Configuration data for ADCON1 register
        ui16ADCON2,                     // Configuration data for ADCON2 register
        ui16ADCON3,                     // Configuration data for ADCON3 register
        ui16ADPinConfig,                // Configuration data for ADPCFG (pin 
                                        //   configuration)
        ui16ScanSelect;                 // Configuration data for ADCSSL (channel
                                        //   scan selection)
  
    //  First, disable interrupts so we can
    //  complete the initialization without
    //  an interruption
    // INTCON1bits.GIE=0;

	//  First, turn off Timers and its associated
    //  interrupt so we can complete the initialization
    //  without being interrupted
    CloseTimer1();
    CloseTimer3();
	CloseADC12();
	
	TRISBbits.TRISB2 = 1;
	TRISBbits.TRISB3 = 0;
	TRISBbits.TRISB4 = 0;
	TRISBbits.TRISB5 = 0;
	TRISBbits.TRISB6 = 0;
	TRISBbits.TRISB7 = 0;
	TRISBbits.TRISB8 = 0;
//	TRISBbits.TRISB9 = 0;
//	TRISFbits.TRISF4 = 0;
//	TRISFbits.TRISF5 = 0;
	TRISFbits.TRISF6 = 0;	
	TRISDbits.TRISD8 = 0;
	TRISDbits.TRISD9 = 0;
	TRISCbits.TRISC13 = 0;	
	TRISCbits.TRISC14 = 0;	
	

    //  Setup the ADCON1 register configuration
    //  data, of which the most important is the
    //  data format, the clock source, and the
    //  Auto Sampling mode
    ui16ADCON1 = ADC_MODULE_ON             // Enable the ADC module
                 & ADC_IDLE_CONTINUE             // Stop conversions during IDLE state
                 & ADC_FORMAT_SIGN_FRACT     // Generate int data
                 & ADC_CLK_AUTO               // Automatically trigger conversions
                 & ADC_AUTO_SAMPLING_OFF      // disable the Auto Sampling mode
    			 //& ADC_SAMP_ON
				;
    //  Setup the ADCON2 register configuration
    //  data, which includes the ADC reference
    //  voltages, the number of samples between
    //  interrupts, and the buffer data order
    //  specification.
    ui16ADCON2 = ADC_VREF_AVDD_AVSS        // Use AVdd and AVss as ADC reference
                 & ADC_SCAN_ON               // Enable channel scan mode
                 & ADC_SAMPLES_PER_INT_1     // Gather 1 sample before interrupting
                 & ADC_ALT_BUF_OFF           // Don't alternate the sample buffer
                 & ADC_ALT_INPUT_OFF         // Don't interleave the input
               	;
    //  Setup the ADCON3 register configuration
    //  data, which includes
    
    ui16ADCON3 = ADC_SAMPLE_TIME_1  // Sample for 1 Tad
//                & ADC_CONV_CLK_SYSTEM   // Use system clock for conversions
                 & ADC_CONV_CLK_INTERNAL_RC   // Use RC (1.5us) clk
//                & ADC_CONV_CLK_32Tcy;    // Allow 32/2 Tcy for conversion
				;
    //  Setup the ADCSSL register configuration
    //  data, which specifies which channels are to
    //  be scanned by the ADC.  Note that the way
    //  the library is implemented, we specify the
    //  ANx signals that we do NOT want to scan in
    //  this list.  In this case, we leave out AN7
    //  since that's the only one we want to sample.
    ui16ScanSelect = SKIP_SCAN_AN0  & 
					SKIP_SCAN_AN1  & 
					// SKIP_SCAN_AN2  & 
                    SKIP_SCAN_AN3  & 
					SKIP_SCAN_AN4  & 
					SKIP_SCAN_AN5  & 
                    SKIP_SCAN_AN6  &
					SKIP_SCAN_AN7;
                     
    //  Make sure that the pins associated with
    //  the analog channel(s) we're using have 
    //  been configured as analog inputs.  This
    //  application uses AN7 as the signal input,
    //  and the dsPICDEM board dedicates AN0 and
    //  AN1 to the ICD2 interface, AN3 to the 
    //  digital potentiometer input, AN4-AN6 to
    //  the analog potentiometer inputs, and AN8
    //  to the temperature sensor input.
    ui16ADPinConfig = ENABLE_AN2_ANA;
    
    //  Actually configure the ADC module.  Note
    //  that this will enable the ADC module, but
    //  the associated interrupt must be enabled
    //  by a call to EnableIntADC() after the rest
    //  of the system has been initialized
    OpenADC12(ui16ADCON1, ui16ADCON2, ui16ADCON3, ui16ADPinConfig,
              ui16ScanSelect);


 	/***** timer1 *****/
	// PWM
	ConfigIntTimer1(
		T1_INT_PRIOR_6 &
		T1_INT_ON
		);
	OpenTimer1(
		T1_ON &
		T1_IDLE_CON &
		T1_GATE_OFF &
		T1_PS_1_1 &
		T1_SYNC_EXT_OFF &
		T1_SOURCE_INT
		,
		//4600 // 312/2usec  for 7.37M crystal @ XT 16x pll
		// 1150 // 312/2usec  for 7.37M crystal @ XT 4x pll
		1150 // 312/2usec  for FRC 4x pll
		//2300
		//3066 //osc2pwm_09
		//1300 //good
		);
	WriteTimer1(0);

 	/***** timer3 *****/
	// data sampling
	ConfigIntTimer3(
		T3_INT_PRIOR_7 &
		T3_INT_ON
		);
	OpenTimer3(
		T3_ON &
		T3_IDLE_CON &
		T3_GATE_OFF &
		T3_PS_1_8 &
		T3_SOURCE_INT
		,
		//3614
		//6000
		//4798 // stopwatch test
		// 10000 //best empirical power test
		//14400 // theoretical value for 7.37M crystal @ XT 16x pll
		// 3600  // theoretical value for 7.37M crystal @ XT 4x pll
		3530  // empirical value for FRC 4x pll
		);
	WriteTimer3(0);

	
	/***** Initialize hanningWin and twiddleFactors *****/
	HanningInit(FFT_BLOCK_LENGTH, &hanningWin[0]);

#ifndef FFTTWIDCOEFFS_IN_PROGMEM					/* Generate TwiddleFactor Coefficients */
	TwidFactorInit (LOG2_BLOCK_LENGTH, &twiddleFactors[0], 0);	/* We need to do this only once at start-up */
#endif
	/****************************************************/
	
	/***** enable interrupts *****/
	ADCON1bits.SAMP=1;
	EnableIntT1;
	EnableIntT3;
    
	// INTCON1bits.GIE=1;                // Enable all interrupts above priority
                                        //   level 0 (i.e., all interrupts)
}