#include "hw.h"
#include <avr/io.h>
#include <avr/interrupt.h>
#include <stdlib.h>
#include "hbw.h"

static uint16_t crc16shift(uint16_t crc, uint8_t data)
{
        int stat, i;

        for (i=0; i<8; i++) {
                stat = crc & 0x8000;
                crc = crc << 1;
                if (data & 0x80)
                        crc = crc | 1;

                if (stat)
                        crc = crc ^ 0x1002;

                data = data << 1;
        }
        return crc;
}

void hbw_send(uint8_t i,uint8_t crc)
{
	if (bit_is_set(UCSR0A, UDRE0)) {
		if (sesc) {
			UDR0 = i & 0x7F;
			sind++;
			return;
		}
		if (crc == 1) {
			scrc = crc16shift(scrc, i);
		}
		if (crc == 2) {
			scrc = crc16shift(scrc, 0);
			scrc = crc16shift(scrc, 0);
			i = (scrc >> 8) & 0xFF;
		}
		if ((i == 0xFD) || (i == 0xFC) || (i == 0xFE)) {
			UDR0 = 0xFC;
			sesc = 1;
			return;
		}
		UDR0 = i;
		sind++;
	}
}

void hbw_loop(void)
{
	if (!sind)
		return;

	if (sdisc) {
		if (sind == 1) {
			hbw_send(0x04,1);
			return;
		}
		if (sind == 2) {
			hbw_send(0x01,1);
			return;
		}
		if (sind == 3) {
			hbw_send((scrc >> 8) & 0xFF,2);
			return;
		}
		if (sind == 4) {
			hbw_send(scrc & 0xFF,0);
			return;
		}
		if (bit_is_set(UCSR0A, TXC0)) {
			PORTD  &= ~(1<<SenderEnable);
			PORTD  &= ~(1<<ReceiverDisable);
			sind = 0;
			sdisc = 0;
		}
		return;
	}

	if (sind < 5) {
		hbw_send(saddress[sind-1],1);
		return;
	}
	if (sind == 5) {
		hbw_send(sctl,1);
		return;
	}
	if (sind < 10) {
		hbw_send(my_address[sind-6],1);
		return;
	}
	if (sind == 10) {
		hbw_send(slen+2,1);
		return;
	}
	if (sind < slen+10) {
		hbw_send(smessage[sind-11],1);
		return;
	}
	if (sind == slen+10) {
		hbw_send(smessage[sind-11],1);
		return;
	}
	if (sind == slen+11) {
		hbw_send((scrc >> 8) & 0xFF,2);
		return;
	}
	if (sind == slen+12) {
		hbw_send(scrc & 0xFF,0);
		return;
	}
	if (bit_is_set(UCSR0A, TXC0)) {
		PORTD  &= ~(1<<SenderEnable);
		PORTD  &= ~(1<<ReceiverDisable);
		sind = 0;
	}
}

char sendmsg(void)
{
	if (sind)
		return 0;

	PORTD  |= (1<<SenderEnable);
	PORTD  |= (1<<ReceiverDisable);
	UCSR0A  = (1<<TXC0);

	scrc = crc16shift(0xFFFF, 0xFD);
	UDR0 = 0xFD;
	sind = 1;
	sesc = 0;
	sdisc = 0;
	return ~0;
	rwant = 20 + (rand() & 0x0F);
}

char senddsc(void)
{
	if (sind)
		return 0;

	PORTD  |= (1<<SenderEnable);
	PORTD  |= (1<<ReceiverDisable);
	UCSR0A  = (1<<TXC0);

	scrc = crc16shift(0xFFFF, 0xFE);
	UDR0 = 0xFE;
	sind = 1;
	sesc = 0;
	sdisc = 1;
	return ~0;
}

char is_bus_free(void)
{

	if(rlast < rwant)
		return 0;
	return 1;
}

void announce(void)
{
        sctl = 0xF8;

        saddress[0] = 0xFF;
        saddress[1] = 0xFF;
        saddress[2] = 0xFF;
        saddress[3] = 0xFF;

        slen = 16;

        smessage[0] = 0x41;
        smessage[1] = 0;
        smessage[2] = HBWTYPE;
        smessage[3] = 0;
        smessage[4] = HBWMAJOR;
        smessage[5] = HBWMINOR;
        smessage[6] = HBWSERIAL0;
        smessage[7] = HBWSERIAL1;
        smessage[8] = HBWSERIAL2;
        smessage[9] = HBWSERIAL3;
        smessage[10] = HBWSERIAL4;
        smessage[11] = HBWSERIAL5;
        smessage[12] = HBWSERIAL6;
        smessage[13] = HBWSERIAL7;
        smessage[14] = HBWSERIAL8;
        smessage[15] = HBWSERIAL9;
	while (!is_bus_free()) ;
        sendmsg();
}

void init_hbw(void)
{
	DDRD |= (1<<SenderEnable);
	DDRD |= (1<<ReceiverDisable);

	PORTD  &= ~(1<<SenderEnable);
	PORTD  &= ~(1<<ReceiverDisable);

	UBRR0L = ((F_CPU/(BAUDRATE*16))-1) & 0xFF;
	UBRR0H = 0;
	UCSR0B = (1 << TXEN0) | (1 << RXEN0) | (1 << RXCIE0);
	UCSR0C = (1 << UPM01) | (1 << UCSZ01) | (1 << UCSZ00);

        OCR2A = 0x96;
        TIMSK2 |= (1 << OCIE2A);
        TCCR2A |= (1 << WGM21);
        TCCR2B = TCCR2B | (1 << CS21) | (1 << CS20);

	rind = 0;
	sind = 0;
	sdisc = 0;
	rready = 0;
	rlast = 0;

	srand(my_address[3]);
	rwant = 20 + (rand() & 0x0F);
	sei();
	announce();
}

ISR(USART_RX_vect)
{
	uint8_t c;

	rlast = 0;

	c = UDR0;
	if (c == 0xFE) {
		rind = 255;
		resc = 0;
	}
	if (c == 0xFD) {
		rind = 0;
		resc = 0;
		rready = 0;
		rcrc =0xFFFF;
	}
	if (rind < 255) {
		if (c == 0xFC) {
			resc = 1;
			return;
		}
		if (resc)
			c = c | 0x80;
		rcrc = crc16shift(rcrc, c);
		resc = 0;
		rind++;
	}
	if ((rind == 1) || (rind == 255))
		return;
	if (rind < 6) {
		rdest[rind-2] = c;
		return;
	}
	if (rind == 6) {
		rctl = c;
		return;
	}
	if (rind < 11) {
		raddress[rind-7] = c;
		return;
	}
	if (rind == 11) {
		rlen = c;
		return;
	}
	if ((rlen < 2) || (rlen > 64)) {
		rind = 255;
		return;
	}
	if ((rind < rlen + 10)) {
		rmessage[rind-12] = c;
		return;
	}
	if (rind == rlen + 10)
		return;
	if (rind == rlen + 11) {
		rind = 255;
		if (rcrc)
			return;
		rready = 1;
		return;
	}
}


ISR(TIMER2_COMPA_vect)
{
	cli();
	if (rlast < rwant)
		rlast++;
	sei();
}