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

#include "buffer.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 = 180 * 102 / 34; 
volatile float setpoint_2 = 150 * 102 / 34;
volatile float setpoint_3 = 150 * 102 / 34;

struct pid {
    // Controller gains
    float kP;
    float kI;
    float kD;

    // State variables
    float lastError;
    float integral;
};

float pid_step(volatile struct pid* controller, float dt, float error) {
    // Calculate p term
    float p = error * controller->kP;

    // Calculate i term
    ATOMIC_BLOCK(ATOMIC_FORCEON){
        controller->integral += error * dt * controller->kI;
        if(controller->integral > 80)
            controller->integral = 80;
        if(controller->integral < -80)
            controller->integral = -80;
    }

    // Calculate d term, taking care to not divide by zero
    float d = dt == 0 ? 0 : ((error - controller->lastError) / dt) * controller->kD;
    controller->lastError = error;

    return p + controller->integral + d;
}

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);
    controller = (struct pid){
        .kP = 5,
        .kI = 0.2,
        .kD = 0,
        .lastError = 0,
        .integral = 0
    };
    //regler
    TCCR1B|=(1<<CS12) | (1<<CS10);
    //TCCR1B|=(1<<WGM12);
	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:
                modbusExchangeRegisters((uint16_t *)&OCR2B,0,1);
                break;
            case fcPresetSingleRegister:
			case fcPresetMultipleRegisters: 
				modbusExchangeRegisters((uint16_t *)&OCR2B,0,1);
                break;

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

            case fcReadInputRegisters:
                if(modbusRequestedAddress()<10){
                    //;
                    //float tmp[4];
                    //tmp[0]=adc_avrg[1]*34/102;
                    //tmp[1]=adc_avrg[0]*34/102;
                    //tmp[2]=adc_avrg[3]*34/102;
                    //tmp[3]=adc_avrg[2]*34/102;
                    modbusExchangeRegisters(&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, 180.0 - 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;
 

    //if(adc_avrg[0] < 99 && !(PORTB & 0x01))
    //    PORTB |= 1 << 0;
    //if(adc_avrg[0] > 99 && (PORTB & 0x01))
    //    PORTB &= ~(1 << 0);
}
ISR(TIMER1_COMPA_vect) {
    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 *= 10;
    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];
            tmp += get_buffer_mean(&adc_buf[current_channel])/30;
            adc_avrg[current_channel] = tmp/100;
        }
        else{
            adc_avrg[current_channel] = get_buffer_mean(&adc_buf[current_channel])/30;
            init[current_channel]=0;
        }
    }

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


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