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

#include "buffer.h"
#include "pid.h"
#include "adc.h"
#include "i2cmaster.h"

void modbusGet(void);


volatile float output;
volatile struct pid controller;

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

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();
    i2c_init();
    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);
}