#include <BeagleRT.h> 
#include <Scope.h>
#include <cmath>
#include <Utilities.h>

float gPhase1, gPhase2;
float gFrequency1, gFrequency2;
float gInverseSampleRate;

Scope scope;    //create a scope object

// initialise_render() 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.
#include <I2c_Codec.h>
#include <PRU.h>
extern I2c_Codec *gAudioCodec;
extern PRU *gPRU;
float D=5264;
#define delayLength 512
float delay[delayLength];
int writePointer=0;
int readPointer=writePointer+1;
AuxiliaryTask updatePll;

void updatePllFunction(){
//	rt_printf("now\n");
	gPRU->setGPIOTestPin();
	gAudioCodec->setPllD(D);
	gPRU->clearGPIOTestPin();
}

bool setup(BeagleRTContext *context, void *userData)
{
	scope.setup(context->audioSampleRate);  //call this once in setup to initialise the scope
	 
	gInverseSampleRate = 1.0/context->audioSampleRate;
	
	gPhase1 = 0.0;
	gPhase2 = 0.0;
	
	gFrequency1 = 200.0;
	gFrequency2 = 201.0;
	updatePll=BeagleRT_createAuxiliaryTask(&updatePllFunction, 98, "update PLL");
	for(int n=0; n<delayLength; n++){
		delay[n]=0;
	}
	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, numMatrixFrames
// will be 0.

void render(BeagleRTContext *context, void *userData)
{
	static int count=0;
	static float lfoPhase=0;
	static float feedback=0;
	int updateRate=8;
	if((count&(updateRate-1))==0 && digitalReadFrame(context,0,P8_07)==GPIO_HIGH){
		float amplitude=context->analogIn[0]/0.84*4990;
		float rate=context->analogIn[1]*20+0.1;
		lfoPhase+=rate*2*M_PI*updateRate*context->analogFrames/context->audioSampleRate;
		D=amplitude+amplitude*sinf(lfoPhase);
		BeagleRT_scheduleAuxiliaryTask(updatePll);
		if((count&255)==0){
			rt_printf("gpio: %d\n",digitalReadFrame(context,0,P8_07));
			rt_printf("D: %.0f\n", D);
			rt_printf("rate: %f\n", rate/2);
			rt_printf("amplitude: %.3f\n", amplitude);
			rt_printf("feedback: %.3f\n\n", feedback);
		}
	}
	count++;

	for(unsigned int n = 0; n < context->audioFrames; n++) {
		feedback=context->analogIn[n/2*context->analogChannels+2]/0.84*1.2;
		if(digitalReadFrame(context,n,P8_08)==GPIO_LOW)
			feedback=0;
	    delay[writePointer++]=context->audioIn[n*context->audioChannels+0] + delay[readPointer]*feedback;
		context->audioOut[n*context->audioChannels+0]=context->audioIn[n*context->audioChannels+0]+delay[readPointer++];
//		context->audioOut[n*context->audioChannels+1]=sinf(gPhase1);
		context->analogOut[n/2*context->analogChannels+0]=D/10000;
		if(writePointer>=delayLength)
			writePointer-=delayLength;
		if(readPointer>=delayLength)
			readPointer-=delayLength;

		gPhase1 += 2.0 * M_PI * gFrequency1 * gInverseSampleRate;
	    gPhase2 += 2.0 * M_PI * gFrequency2 * gInverseSampleRate;
		if(gPhase1 > 2.0 * M_PI)
			gPhase1 -= 2.0 * M_PI;
		if(gPhase2 > 2.0 * M_PI)
			gPhase2 -= 2.0 * M_PI;
		
	}
}

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

void cleanup(BeagleRTContext *context, void *userData)
{
    
}
