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

#include "yaMBSiavr.h"

uint8_t font[] = {
    0xEE,
    0x82,
    0xDC,
    0xD6,
    0xB2,
    0x76,
    0x7E,
    0xC2,
    0xFE,
    0xF6,
};

uint8_t display[4];
volatile uint16_t holdingRegisters[4];

void timer0_init()
{
    TCCR0 = (1<<WGM01); //TIMER0 SET-UP: CTC MODE
    TCCR0 = (1<<CS01)|(1<<CS00); // PS 1:64
    OCR0 = 249; // 1ms reach for clear (16mz:64=>250kHz:250-=>1kHz)
    TIMSK |= 1<<OCIE0;     //IRQ on TIMER0 output compareA
}

void timer2_init(void) {
    //TCCR2 = (1<<WGM21); //TIMER0 SET-UP: CTC MODE
    //TCCR2 = (1<<CS21)|(1<<CS20); // PS 1:64
    //OCR2 = 25; // 1ms reach for clear (16mz:64=>250kHz:250-=>1kHz)
    //TIMSK |= 1<<OCIE2;     //IRQ on TIMER0 output compareA

	TCCR2|=(1<<CS21); //prescaler 8
	TIMSK|=(1<<TOIE2);

}

unsigned long bin2bcd (uint32_t n)     // convert 16 bit binary n to 5 digit BCD (packed)
{
    return n + 6 * (n/10 + 16*(n/100) + 256*(n/1000) + 4096*(n/10000));
}

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

                uint8_t h = holdingRegisters[0] / 3600;
                uint8_t m = (holdingRegisters[0] / 60) % 60;
                uint8_t s = holdingRegisters[0] % 60;

                uint16_t bcd;
                if(h)
                    bcd = bin2bcd(m) | bin2bcd(h)<<8;
                else
                    bcd = bin2bcd(s) | bin2bcd(m)<<8;

                display[0] = font[(bcd&0x000f)];
                display[1] = font[(bcd&0x00f0) >> 4];

                display[2] = font[(bcd&0x0f00) >> 8];
                display[3] = bcd&0xF000 ? font[(bcd&0xf000) >> 12] : 0;
                break;
            default:
                modbusSendException(ecIllegalFunction);
                break;
        }
    }
}

int main(void){
    DDRB |= _BV(4) | _BV(3) | _BV(2) | _BV(1) | _BV(0);
    DDRD = 0xFC;
    DDRA = 0x03 << 4;
    DDRC = 0xFE;

	modbusSetAddress(24); //better set this to sth.
	modbusInit();

    timer0_init();
    timer2_init();

    sei();

    while(1){
        modbusGet();
    }

}

ISR(TIMER0_COMP_vect)                             
{
    if(PORTB & 0x01){
        PORTB = (PORTB & 0xF0) | 1<<1 | 1<<3;
        PORTD = display[3] & 0xFC;
        PORTA = (PORTA & 0xCF) | (display[3] & 0x03) << 4;
        PORTC = display[1];
    }
    else{
        PORTB = (PORTB & 0xF0) | 1<<0 | 1<<2;
        PORTD = display[2] & 0xFC;
        PORTA = (PORTA & 0xCF) | (display[2] & 0x03) << 4;
        PORTC = display[0];
    }
}

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