#include "filter.h"
#include <math.h>
#include "debug.h"

Sensordata sensordata[4];
float ambient_temp=25.;


float avg_cumul[CHANNELCOUNT];
float avg_lastval[CHANNELCOUNT];
uint8_t avg_count[CHANNELCOUNT];
float avg_noise[CHANNELCOUNT];


void process_thermocouple_value(int16_t raw_data, uint8_t channel){
//    dbgLog("procthermval, val %i, chan %i",raw_data,channel);
    // This function does everything that needs to be done to raw adc values
    float a = raw_data;
    a -= sensordata[channel].offset; 
    a = a*0.000625;  // adc count to voltage (mV)
    a = filter_voltage_to_temp(a); // voltage to temp
    // TODO: first ambient->voltage, add that to a, then voltage->temp
    a += ambient_temp; // compensate ambient temperature
    // TODO: lowpass
    
    uint16_t result = filter_float_to_fixpoint(a);
    sensordata[channel].temperature = result;
}

void process_offset_value(int16_t raw_data, uint8_t channel){
    sensordata[channel].offset = raw_data;
}

void process_ambient_value(int16_t raw_data){
    // 9.625mV after amp  per °C
    float a = raw_data;
    a = a * 0.03125; //adc to mV after amp
    a = a / 9.625;
    ambient_temp = a;
    dbgLog("ambient: %3.3f°C (value %i)\n",a, raw_data);
}


uint16_t get_temperature(uint8_t channel){
    if(channel>=4){
        return 0;
    }
    return sensordata[channel].temperature;
}


void filter_average_input(uint8_t channel, float value) {
	avg_cumul[channel] += value;
	float noisetmp = avg_lastval[channel] - value;	
	avg_noise[channel] += noisetmp * noisetmp;
	avg_count[channel] += 1;
}

uint8_t filter_average_done(uint8_t channel, uint8_t samples){
	if(avg_count[channel] >= samples){
		return(1);
	}
	return 0;
}

float filter_average_temp_result(uint8_t channel){
	float out = avg_cumul[channel];
	out = out / (float)avg_count[channel];
	return out;
}

float filter_average_result(uint8_t channel){
	float out = filter_average_temp_result(channel);
	avg_lastval[channel] = out;
	avg_cumul[channel] = 0;
	avg_noise[channel] = 0;
	avg_count[channel] = 0;
	return out;
}

float filter_average_noise(uint8_t channel){
	float noise = avg_noise[channel];
	noise /= avg_count[channel];
	return noise;
}

uint16_t filter_float_to_fixpoint(float f){
	return( (uint16_t)(f * 256.) );
}

//uint16_t calc_temp(int16_t val, uint8_t channel) {
//	float mV =  ((float) avg_temp(val, channel))/1000000;
float filter_temp_to_voltage(float temp){
	float mV = 0;
	if(temp < 0) {
		/* t in temp
		E = sum(i=0 to n) c_i t^i.		; n=10
		*/
		const static float coef[] = {
			0.000000000000E+00,
			0.394501280250E-01,
			0.236223735980E-04,
			-0.328589067840E-06,
			-0.499048287770E-08,
			-0.675090591730E-10,
			-0.574103274280E-12,
			-0.310888728940E-14,
			-0.104516093650E-16,
			-0.198892668780E-19,
			-0.163226974860E-22
		};
		for(int i=0; i<11; i++) {
			mV += coef[i] * pow(temp, i);
		}
	} else {
		/*
		  t in temp
		  E = sum(i=0 to n) c_i t^i + a0 exp(a1 (t - a2)^2).     ; n = 9
		 */
		const static float coef[] = {
			-0.176004136860E-01,
			0.389212049750E-01,
			0.185587700320E-04,
			-0.994575928740E-07,
			0.318409457190E-09,
			-0.560728448890E-12,
			0.560750590590E-15,
			-0.320207200030E-18,
			0.971511471520E-22,
			-0.121047212750E-25
		};
		const static float a[] = {0.118597600000E+00, -0.118343200000E-03, 0.126968600000E+03};
		for(int i=0; i<10; i++) {
			mV += coef[i]*pow(temp, i);
		}
		// ich glaube das summenzeichen erstreckt sich nicht auf das exp
		mV += a[0] * pow(exp(a[1]*(temp - a[2])), 2);
		
	}
	return mV;
}





float filter_voltage_to_temp(float mV){
	//TODO: evtl kennlinien für <0°C und >500°C
	
	// Kennlinen aus http://srdata.nist.gov/its90/download/type_k.tab
	float temp=0;
	const static float coef[] = {
		  0.000000E+00,
		  2.508355E+01,
		  7.860106E-02,
		 -2.503131E-01,
		  8.315270E-02,
		 -1.228034E-02,
		  9.804036E-04,
		 -4.413030E-05,
		  1.057734E-06,
		 -1.052755E-08,
	};
	for(int i=0; i<10; i++) {
		temp += coef[i] * pow(mV, i);
	}
	return temp;
}