/*
 ____  _____ _        _    
| __ )| ____| |      / \   
|  _ \|  _| | |     / _ \  
| |_) | |___| |___ / ___ \ 
|____/|_____|_____/_/   \_\

The platform for ultra-low latency audio and sensor processing

http://bela.io

A project of the Augmented Instruments Laboratory within the
Centre for Digital Music at Queen Mary University of London.
http://www.eecs.qmul.ac.uk/~andrewm

(c) 2016 Augmented Instruments Laboratory: Andrew McPherson,
  Astrid Bin, Liam Donovan, Christian Heinrichs, Robert Jack,
  Giulio Moro, Laurel Pardue, Victor Zappi. All rights reserved.

The Bela software is distributed under the GNU Lesser General Public License
(LGPL 3.0), available here: https://www.gnu.org/licenses/lgpl-3.0.txt
*/


#include <Bela.h>	// to schedule lower prio parallel process
#include <rtdk.h>
#include <cmath>
#include <stdio.h>
#include "SampleData.h"

SampleData gSampleData;	// User defined structure to get complex data from main
int gReadPtr;			// Position of last read sample from file

// filter vars
float gLastX[2];
float gLastY[2];
double lb0, lb1, lb2, la1, la2 = 0.0;

// communication vars between the 2 auxiliary tasks
int gChangeCoeff = 0;
int gFreqDelta = 0;

void initialise_filter(float freq);

void calculate_coeff(float cutFreq);

bool initialise_aux_tasks();

// Task for handling the update of the frequencies using the matrix
AuxiliaryTask gChangeCoeffTask;

void check_coeff();

// Task for handling the update of the frequencies using the matrix
AuxiliaryTask gInputTask;

void read_input();


extern float gCutFreq;


bool setup(BelaContext *context, void *userData)
{

	// Retrieve a parameter passed in from the initAudio() call
	gSampleData = *(SampleData *)userData;

	gReadPtr = -1;

	initialise_filter(200);

	// Initialise auxiliary tasks
	if(!initialise_aux_tasks())
		return false;

	return true;
}

void render(BelaContext *context, void *userData)
{
	for(unsigned int n = 0; n < context->audioFrames; n++) {
		float sample = 0;
		float out = 0;

		// If triggered...
		if(gReadPtr != -1)
			sample += gSampleData.samples[gReadPtr++];	// ...read each sample...

		if(gReadPtr >= gSampleData.sampleLen)
			gReadPtr = -1;

		out = lb0*sample+lb1*gLastX[0]+lb2*gLastX[1]-la1*gLastY[0]-la2*gLastY[1];

		gLastX[1] = gLastX[0];
		gLastX[0] = out;
		gLastY[1] = gLastY[0];
		gLastY[0] = out;

		for(unsigned int channel = 0; channel < context->audioChannels; channel++)
			context->audioOut[n * context->audioChannels + channel] = out;	// ...and put it in both left and right channel

	}

	// Request that the lower-priority tasks run at next opportunity
	Bela_scheduleAuxiliaryTask(gChangeCoeffTask);
	Bela_scheduleAuxiliaryTask(gInputTask);
}

// First calculation of coefficients

void initialise_filter(float freq)
{
	calculate_coeff(freq);
}


// Calculate the filter coefficients
// second order low pass butterworth filter

void calculate_coeff(float cutFreq)
{
	// Initialise any previous state (clearing buffers etc.)
	// to prepare for calls to render()
	float sampleRate = 44100;
	double f = 2*M_PI*cutFreq/sampleRate;
	double denom = 4+2*sqrt(2)*f+f*f;
	lb0 = f*f/denom;
	lb1 = 2*lb0;
	lb2 = lb0;
	la1 = (2*f*f-8)/denom;
	la2 = (f*f+4-2*sqrt(2)*f)/denom;
	gLastX[0] = gLastX [1] = 0;
	gLastY[0] = gLastY[1] = 0;

}


// Initialise the auxiliary tasks
// and print info

bool initialise_aux_tasks()
{
	if((gChangeCoeffTask = Bela_createAuxiliaryTask(&check_coeff, 90, "bela-check-coeff")) == 0)
		return false;

	if((gInputTask = Bela_createAuxiliaryTask(&read_input, 50, "bela-read-input")) == 0)
		return false;

	rt_printf("Press 'a' to trigger sample, 's' to stop\n");
	rt_printf("Press 'z' to low down cut-off freq of 100 Hz, 'x' to raise it up\n");
	rt_printf("Press 'q' to quit\n");

	return true;
}

// Check if cut-off freq has been changed
// and new coefficients are needed

void check_coeff()
{
	if(gChangeCoeff == 1)
	{
		gCutFreq += gFreqDelta;
		gCutFreq = gCutFreq < 0 ? 0 : gCutFreq;
		gCutFreq = gCutFreq > 22050 ? 22050 : gCutFreq;

		rt_printf("Cut-off frequency: %f\n", gCutFreq);

		calculate_coeff(gCutFreq);
		gChangeCoeff = 0;
	}
}

// This is a lower-priority call to periodically read keyboard input
// and trigger samples. By placing it at a lower priority,
// it has minimal effect on the audio performance but it will take longer to
// complete if the system is under heavy audio load.

void read_input()
{
	// This is not a real-time task!
	// Cos getchar is a system call, not handled by Xenomai.
	// This task will be automatically down graded.

	char keyStroke = '.';

	keyStroke =	getchar();
	while(getchar()!='\n'); // to read the first stroke

	switch (keyStroke)
	{
		case 'a':
			gReadPtr = 0;
			break;
		case 's':
			gReadPtr = -1;
			break;
		case 'z':
			gChangeCoeff = 1;
			gFreqDelta = -100;
			break;
		case 'x':
			gChangeCoeff = 1;
			gFreqDelta = 100;
			break;
		case 'q':
			gShouldStop = true;
			break;
		default:
			break;
	}
}


void cleanup(BelaContext *context, void *userData)
{
	delete[] gSampleData.samples;
}


/**
\example filter-IIR/render.cpp

Infinite Impulse Response Filter
------------------------------

This is an example of a infinite impulse response filter implementation.
*/
