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

uint16_t inputStatus[9];

void init_clk(void)
{
    // ========= System Clock configuration =========
    // Set to external 16Mhz crystal, using the PLL at *2
    // set it to be a 12-16Mhz crystal with a slow start-up time.
    OSC.XOSCCTRL = OSC_FRQRANGE_2TO9_gc | OSC_XOSCSEL_XTAL_16KCLK_gc ;
    OSC.CTRL |= OSC_XOSCEN_bm ; // enable it
    while( (OSC.STATUS & OSC_XOSCRDY_bm) == 0 ){} // wait until it's stable

    // The external crystal is now running and stable.
    // (Note that it's not yet selected as the clock source)
    // Now configure the PLL to be eXternal oscillator * 2
    OSC.PLLCTRL = OSC_PLLSRC_XOSC_gc | 2;

    // now enable the PLL...
    OSC.CTRL |= OSC_PLLEN_bm ; // enable the PLL...
    while( (OSC.STATUS & OSC_PLLRDY_bm) == 0 ){} // wait until it's stable

    // And now, *finally*, we can switch from the internal 2Mhz clock to the PLL
    CCP = CCP_IOREG_gc;	// protected write follows   
    CLK.CTRL = CLK_SCLKSEL_PLL_gc;	// The System clock is now  PLL (16Mhz * 2)
    // ==============================================
}

void init_timer_100us(){
    TCC0.CTRLA |= TC_CLKSEL_DIV256_gc;
    TCC0.PER = 13; // ==> 9615us
    TCC0.INTCTRLA |= TC_OVFINTLVL_MED_gc;
}

void modbusGet(void) {
	if (modbusGetBusState() & (1<<ReceiveCompleted))
    {
        switch(rxbuffer[1]) {
            case fcReadHoldingRegisters:
                inputStatus[0]=(~PORTC.IN) & 0xFF;
                modbusExchangeRegisters(inputStatus, 0, 8);
                break;
            default:
                modbusSendException(ecIllegalFunction);
                break;
        }
    }
}

int main(void){
    init_clk();
    PORTD.DIRSET = 1 << 5;

    PORTCFG.MPCMASK = 0xFF;
    PORTC.PIN1CTRL = PORT_OPC_PULLUP_gc;

    init_timer_100us();

    PMIC.CTRL |= PMIC_LOLVLEN_bm | PMIC_MEDLVLEN_bm | PMIC_HILVLEN_bm; 
    sei();

    modbusSetAddress(1);
    modbusInit();

    for(;;){
        modbusGet();
    }
}

ISR(TCC0_OVF_vect){
    modbusTickTimer();
}