#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"
#include <util/delay.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(1);

	int16_t data;
	for(;;){

		// measure temps 5*2 times
		dbgLog("starting temp measuring");
		for(int i=0; i<5; i++) {
			for(uint8_t active_sensor=0; active_sensor<2; active_sensor++) { // only measuring two probes atm
				muxer_set(active_sensor);
				while(!mcpadc_has_new_data()) _delay_ms(1);
				mcpadc_get_data(); // first data after switch to trash
				while(!mcpadc_has_new_data()) _delay_ms(1);
				
				data = mcpadc_get_data();

				dbgLog("measured data %i, now filtering",data);
				float f = filter_voltage_to_temp(((float)data) * 0.000625 );
				filter_average_input(active_sensor, f);
				if(filter_average_done(active_sensor,16)){
					filter_average_noise(active_sensor);
					temperatures[active_sensor] = filter_average_result(active_sensor);
					dbgLog("finished filtering, now have %3.3f°C",temperatures[active_sensor]);

				}
			}
		}

		dbgLog("now measuring offsets");
		// measure 2 offsets
		for(uint8_t active_sensor=0; active_sensor<2; active_sensor++) { // only measuring two offsets atm
			dbgLog("active sensor: %i",active_sensor);
			muxer_set(active_sensor);
			offset_measure_start(active_sensor);
			while(!mcpadc_has_new_data()) _delay_ms(1);
			mcpadc_get_data(); // first data after switch to trash
			while(!mcpadc_has_new_data()) _delay_ms(1);

			data = mcpadc_get_data();
			dbgLog("retrieved offset data %i", data);
			// TODO: what to do with the offset?
			
			offset_measure_stop();
		}
        
        // measure ambient
        muxer_set(5);
        while(!mcpadc_has_new_data()) _delay_ms(1);
        mcpadc_get_data(); // first data after switch to trash
        while(!mcpadc_has_new_data()) _delay_ms(1);
        data = mcpadc_get_data();
        process_ambient_value(data);


/*		SOFTTIMER(1,8000) { // maybe measure coldjunction comp
			
			/* // first measure ntc offset?! maybe? */
			/* offset_measure_start(active_sensor); */
			/* while(!mcpadc_has_new_data()) _delay_ms(1); */
			/* mcpadc_get_data(); // first data after switch to trash */
			/* while(!mcpadc_has_new_data()) _delay_ms(1); */

			/* data = mcpadc_get_data(); */
			/* offset_measure_stop(); */

			/* // now measure temp-data */
			/* while(!mcpadc_has_new_data()) _delay_ms(1); */
			/* mcpadc_get_data(); // first data after switch to trash */
			/* while(!mcpadc_has_new_data()) _delay_ms(1); */

			/* data = mcpadc_get_data();			 */
	}
	
}


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

}