#include <BeagleRT.h>
#include <Utilities.h>
#include <cmath>
#include <rtdk.h>
#include "I2C_MPR121.h"

// How many pins there are
#define NUM_TOUCH_PINS 12

// Define this to print data to terminal
#undef DEBUG_MPR121

// Change this to change how often the MPR121 is read (in Hz)
int readInterval = 50;

// Change this threshold to set the minimum amount of touch
int threshold = 40;

// This array holds the continuous sensor values
int sensorValue[NUM_TOUCH_PINS];

// ---- test code stuff -- can be deleted for your example ----

// 12 notes of a C major scale...
float gFrequencies[NUM_TOUCH_PINS] = {261.63, 293.66, 329.63, 349.23, 392.00, 440.00, 493.88, 523.25, 587.33, 659.25, 698.25, 783.99};

// This is internal stuff for the demo
float gNormFrequencies[NUM_TOUCH_PINS];
float gPhases[NUM_TOUCH_PINS] = {0};

// ---- internal stuff -- do not change -----

I2C_MPR121 mpr121;			// Object to handle MPR121 sensing
AuxiliaryTask i2cTask;		// Auxiliary task to read I2C

int readCount = 0;			// How long until we read again...
int readIntervalSamples = 0; // How many samples between reads

void readMPR121();

// setup() is called once before the audio rendering starts.
// Use it to perform any initialisation and allocation which is dependent
// on the period size or sample rate.
//
// userData holds an opaque pointer to a data structure that was passed
// in from the call to initAudio().
//
// Return true on success; returning false halts the program.

bool setup(BeagleRTContext *context, void *userData)
{
	if(!mpr121.begin(1, 0x5A)) {
		rt_printf("Error initialising MPR121\n");
		return false;
	}
	
	i2cTask = BeagleRT_createAuxiliaryTask(readMPR121, 50, "beaglert-mpr121");
	readIntervalSamples = context->audioSampleRate / readInterval;
	
	for(int i = 0; i < NUM_TOUCH_PINS; i++) {
		gNormFrequencies[i] = 2.0 * M_PI * gFrequencies[i] / context->audioSampleRate;
	}
	
	return true;
}

// render() is called regularly at the highest priority by the audio engine.
// Input and output are given from the audio hardware and the other
// ADCs and DACs (if available). If only audio is available, numAnalogFrames
// will be 0.

void render(BeagleRTContext *context, void *userData)
{
	for(int n = 0; n < context->audioFrames; n++) {
		// Keep this code: it schedules the touch sensor readings
		if(++readCount >= readIntervalSamples) {
			readCount = 0;
			BeagleRT_scheduleAuxiliaryTask(i2cTask);
		}
		
		float sample = 0.0;
		
		// This code can be replaced with your favourite audio code
		for(int i = 0; i < NUM_TOUCH_PINS; i++) {
			float amplitude = sensorValue[i] / 400.0;
			
			// Prevent clipping
			if(amplitude > 0.5)
				amplitude = 0.5;
			
			sample += amplitude * sinf(gPhases[i]);
			gPhases[i] += gNormFrequencies[i];
			if(gPhases[i] > 2.0 * M_PI)
				gPhases[i] -= 2.0 * M_PI;
		}
		
		for(int ch = 0; ch < context->audioChannels; ch++)
			context->audioOut[context->audioChannels * n + ch] = sample;
	}
}

// cleanup() is called once at the end, after the audio has stopped.
// Release any resources that were allocated in setup().

void cleanup(BeagleRTContext *context, void *userData)
{
	// Nothing to do here
}


// Auxiliary task to read the I2C board
void readMPR121()
{
	for(int i = 0; i < NUM_TOUCH_PINS; i++) {
		sensorValue[i] = -(mpr121.filteredData(i) - mpr121.baselineData(i));
		sensorValue[i] -= threshold;
		if(sensorValue[i] < 0)
			sensorValue[i] = 0;
#ifdef DEBUG_MPR121
		rt_printf("%d ", sensorValue[i]);
#endif
	}
#ifdef DEBUG_MPR121
	rt_printf("\n");
#endif
	
	// You can use this to read binary on/off touch state more easily
	//rt_printf("Touched: %x\n", mpr121.touched());
}