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

uint16_t timertmp;
uint16_t temperatures[4];

/* pinout
 * - i2c:
 *		C5
 *		C4
 * - multiplexer:
 *		C3: inhibit
 *		C0-C2: muxer select
 *	- offset measurement
 *	    D0-D3 offset compensation
 *
 *
 * amp 0 is on muxer channel 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();
  offset_measure_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(0);

	uint8_t active_sensor = 0;

	for(;;){
    
		SOFTTIMER(1,10) {
			if(mcpadc_has_new_data()) {
				int16_t data = mcpadc_get_data();
				float f = filter_voltage_to_temp(
						((float)data) * 0.000625 );
				// total gain is 100 (50 from INA, 2 from ADC)
				// full signed range on ADC is +-2.048V
				// with 16bit, 1LSB is worth 0.0625mV
				// with the gain added in that's 0.000625mV

				filter_average_input(active_sensor, f);
				if(filter_average_done(active_sensor,16)){
					float noise = filter_average_noise(active_sensor);
					float temp = filter_average_result(active_sensor);
					dbgLog("temp: %3.3f°C (lastval %i), noise: %e\n",temp, data, noise);
				}
			}
		}

/*		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);
				}
			}
		}
*/
		SOFTTIMER(3,4000){
			static uint8_t toggle;
			if(toggle){
				offset_measure_start(0);
				toggle=0;
				dbgLog("measuring offset\n");
			} else {
				offset_measure_stop();
				toggle=1;
				dbgLog("stopping offset-measuring\n");
			}
		}
    }
}


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;
    }

}