#include <avr/io.h>
#include <util/delay.h>
#include <modbus.h>
#include <avr/interrupt.h>

#include "buffer.h"
#include "pid.h"

void modbusGet(void);

volatile buffer_t adc_buf[4];
volatile float adc_avrg[4];

volatile float output;
volatile struct pid controller;

volatile float setpoint_1 = 130; 
volatile float setpoint_2 = 150;
volatile float setpoint_3 = 150;

void initADC(void)
{
    ADMUX = 1 << REFS0 | 0 << REFS1; //Select external Vref

    //ADC Status Register A
    ADCSRA = 1 << ADEN   //Enable ADC
             | 1 << ADIE //Enable ISR after conversion complete
             //|  1<<ADATE //Freerunning-Mode
             //|  1<<ADLAR //2 results bits are left aligned
             | 1 << ADPS2 //Set clock-prescaler to 128
             | 1 << ADPS1 | 1 << ADPS0;
    ADCSRA |= 1 << ADSC; //Start first Conversion for "warmup"
}

int main(){
    DDRD |= (1 << 4); // LED
    DDRD |= (1 << 3); // FU PWM
    DDRD |= (1 << 2); // 485 DE

    PORTD |= 1 << 4;

    DDRB |= 0x0F; // out channels

	PORTD|=(1<<0); // RX

    // FU PWM on PD3
    TCCR2A |= (1 << COM2B1) | (1 << COM2B0);
    TCCR2A |= (1 << WGM21) | (1 << WGM20);
    TCCR2B |= (1 << CS21);

	modbusSetAddress(1);
    modbusInit();

    // Modbus Tick Timer
    TCCR0B|=(1<<CS01); //prescaler 8
	TIMSK0|=(1<<TOIE0);

    initADC();
    sei();

    _delay_ms(1000);
    init_pid(&controller, 5, 0.2, 0);


    // timer for regler
    TCCR1B|=(1<<CS12) | (1<<CS10);
	TIMSK1|=(1<<TOIE1)|(1<<OCIE1A);

    while(1){
	    modbusGet();
    }
}

void set_ADC_Channel(uint8_t adr)
{
    if (adr < 11)
    {
        ADMUX &= (0b11110000); //Clear MUX-Address
        ADMUX |= adr;          //Set new MUX-Address
    }
}

void modbusGet(void) {
	if (modbusGetBusState() & (1<<ReceiveCompleted))
	{
		switch(rxbuffer[1]) {
            case fcReadHoldingRegisters:
            case fcPresetSingleRegister:
			case fcPresetMultipleRegisters: 
				modbusExchangeRegisters((uint16_t *)&OCR2B,0,1);
                break;

            case fcReadCoilStatus:
            case fcForceSingleCoil:
            case fcForceMultipleCoils:
				modbusExchangeBits(&PORTB,0,4);
                break;

            case fcReadInputRegisters:
                if(modbusRequestedAddress()<10)
                    modbusExchangeRegisters((uint16_t *)&adc_avrg,0,8);
                else
                    modbusExchangeRegisters((uint16_t *)&output,10,4);
                break;
			
            default:
				modbusSendException(ecIllegalFunction);
                break;
		}
	}
}

ISR(TIMER0_OVF_vect) { //this ISR is called 9765.625 times per second
	modbusTickTimer();
}

ISR(TIMER1_OVF_vect) {
    output = pid_step(&controller, 1, setpoint_1 - adc_avrg[1]);

    PORTD &= ~(1 << 4);

    if(output > 128)
        output = 128;

    if(output >= 1){
        uint32_t tmp = output * 512;
        if(tmp >= 0x10000)
            tmp = 0xFFFE;

        OCR1A = tmp;
        PORTB |= 0x0E;
    }
    else
        OCR1A = 1000;
}

ISR(TIMER1_COMPA_vect) {
    /* turn off outputs */
    PORTB &= ~(0x0E);
}

ISR(ADC_vect)
{
    static uint8_t init[4] = {0,0,0,0};
    static uint8_t current_channel = 0;

    //Reading 10bit conversion result
    uint16_t ADC_reading = ADCL;        //copy the first LSB bits
    ADC_reading |= ADCH << 8;           //copy remaing byte
    ADC_reading *= 0.33;
    insert_to_buffer(ADC_reading, &adc_buf[current_channel]);

    if(adc_buf[current_channel].position == BUFFER_SIZE-1){
        if(init[current_channel]){
            float tmp = (99*adc_avrg[current_channel]) + get_buffer_mean(&adc_buf[current_channel]);
            tmp /= 100;
            adc_avrg[current_channel] = tmp;
        }
        else{
            adc_avrg[current_channel] = get_buffer_mean(&adc_buf[current_channel]);
            init[current_channel]=0;
        }
    }

    current_channel++;
    if(current_channel == 4)
        current_channel = 0;
    set_ADC_Channel(current_channel);

    ADCSRA |= (1 << ADSC); //Start next conversion
}