#include "main.h"
#include "spi.h"
#include "muxer.h"
#include "i2c_simple.h"
#include "mcp_adc.h"
#include "debug.h"
#include "math.h"

uint16_t timertmp;
uint16_t temperatures[4];
uint8_t sensor_active[4];
int32_t temp_avg_cumul[4];
int16_t temp_avg_count[4];

/* pinout
 * - i2c:
 *		C5
 *		C4
 * - multiplexer:
 *		C3: inhibit
 *		C0-C2: muxer select
 *
 *
 * amp 0 is on muxer channel 2
 */

#define MUXER_CHANNEL_0 2
#define MUXER_CHANNEL_1 0
#define MUXER_CHANNEL_2 1
#define MUXER_CHANNEL_3 3


int32_t avg_temp(int16_t val, uint8_t channel) {
	int32_t temp = temp_avg_cumul[channel];
	temp /= temp_avg_count[channel];
	temp_avg_count[channel] = 0;
	temp_avg_cumul[channel] = 0;
	return (temp * 625); // nV
}

uint16_t calc_temp(int16_t val, uint8_t channel) {
	float mV =  ((float) avg_temp(val, channel))/1000000;
	uint16_t temp;
	if(mV < 0) {
		/* t in mV
		E = sum(i=0 to n) c_i t^i.		; n=10
		*/
		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++) {
			temp += coef[i] * pow(mV, i+1);
		}
		return temp;
	} else {
		/*
		  t in mV
		  E = sum(i=0 to n) c_i t^i + a0 exp(a1 (t - a2)^2).     ; n = 9
		 */
		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
		};
		float a[] = {0.118597600000E+00, -0.118343200000E-03, 0.126968600000E+03};
		for(int i=0; i<10; i++) {
			temp += coef[i]*pow(mV, i+1) + a[0] * pow(exp(a[1]*(mV - a[2])), 2);
		}
		
	}
}

/* initializes the hardware */
void hardinit() {
  // enable softtimer isr
  TCCR1A = _BV(WGM10);
  TCCR1B = _BV(WGM12) | _BV(CS11); // clk/8 prescaler, 4kHz PWM-freq.
  TIMSK1 |= _BV(TOIE1);

  spi_init();

  muxer_init();
  i2c_init();

  sei();
}

void softinit() {
	mcpadc_init(ADC_GAIN_2|ADC_CONV_CONT|ADC_BITS_16);
}


int __attribute__((noreturn)) main(void) {
  hardinit();
  softinit();

  muxer_set(MUXER_CHANNEL_0);

  for(;;){
    
    SOFTTIMER(1,10) {
		for(int i=0; i<4; i++) {
			if(sensor_active[i] && mcpadc_has_new_data(i)) {
				temperatures[i] = calc_temp( mcpadc_get_data(i), i );
				temp_avg_cumul[i] += temperatures[i];
				temp_avg_count[i] += 1;
			}
		}		
    }

	SOFTTIMER(2,500){
		for(int i=0; i<4; i++) {
			if(sensor_active[i]) {
				int32_t temp = temp_avg_cumul[i];
				temp /= temp_avg_count[i];
				temp_avg_count[i] = 0; temp_avg_cumul[i] = 0;
				int32_t nV = (temp * 625);
				int32_t mK = nV/39;
				dbgLog("temp-%i: %6li (%li µV, %li mK)\n",i,temp,nV/1000, mK);
			}
		}
	}

  }
}


ISR(TIMER1_OVF_vect,ISR_NOBLOCK){
    timertmp=timer1_acc;
    timertmp++;

    /* the ATOMIC is acutally only needed if timer1_acc is never read from an ISR, which
     * is probably the case.
     * ATOMIC_FORCEON: the ISR_NOBLOCK sets sei() a few cycles before.
     */
    ATOMIC_BLOCK(ATOMIC_FORCEON){ 
        timer1_acc=timertmp;
    }

}