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

uint32_t my_address;

uint8_t rind;
uint16_t rcrc;
uint32_t raddress;
uint32_t rdest;
uint8_t resc;
uint8_t rctl;
volatile uint8_t rready;
uint8_t rmessage[62];
uint8_t rlen;

uint8_t sind;
uint8_t sdisc;
uint8_t sctl;
uint16_t scrc;
uint8_t sesc;
uint32_t saddress;
uint8_t slen;
uint8_t smessage[62];
uint8_t slocked;
uint8_t sbusy;

volatile uint16_t sdelay;

uint8_t waitack;
uint32_t aaddress;
uint8_t actl;
uint8_t alen;
uint8_t alink;
uint8_t amessage[62];

uint8_t asent;
uint8_t dsent;
uint8_t gotack;

uint8_t configled;
uint8_t configbutton;
uint8_t configstate;
uint16_t configtimer;

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;
}

static void readaddr(void)
{
	((uint8_t*)&my_address)[0]=eeprom_read_byte((const uint8_t *)EESIZE-4);
	((uint8_t*)&my_address)[1]=eeprom_read_byte((const uint8_t *)EESIZE-3);
	((uint8_t*)&my_address)[2]=eeprom_read_byte((const uint8_t *)EESIZE-2);
	((uint8_t*)&my_address)[3]=eeprom_read_byte((const uint8_t *)EESIZE-1);

	if (my_address == 0xFFFFFFFF)
		((uint8_t*)&my_address)[0] = 0x42;
}

static void send(uint8_t i,uint8_t crc)
{
	if (bit_is_set(UCSR0A, UDRE0)) {
		if (sesc) {
			UDR0 = i & 0x7F;
			sind++;
			sesc = 0;
			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 rloop(void)
{
	if (!sind)
		return;

	if (sdisc) {
		if (sind == 1) {
			send(0x04,1);
			return;
		}
		if (sind == 2) {
			send(0x01,1);
			return;
		}
		if (sind == 3) {
			send((scrc >> 8) & 0xFF,2);
			return;
		}
		if (sind == 4) {
			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) {
		send(((uint8_t*)&saddress)[sind-1],1);
		return;
	}
	if (sind == 5) {
		send(sctl,1);
		return;
	}
	if (sind < 10) {
		send(((uint8_t*)&my_address)[sind-6],1);
		return;
	}
	if (sind == 10) {
		send(slen+2,1);
		return;
	}
	if (sind < slen+10) {
		send(smessage[sind-11],1);
		return;
	}
	if (sind == slen+10) {
		send(smessage[sind-11],1);
		return;
	}
	if (sind == slen+11) {
		send((scrc >> 8) & 0xFF,2);
		return;
	}
	if (sind == slen+12) {
		send(scrc & 0xFF,0);
		return;
	}
	if (bit_is_set(UCSR0A, TXC0)) {
		PORTD  &= ~(1<<SenderEnable);
		PORTD  &= ~(1<<ReceiverDisable);
		sind = 0;
	}
}

static uint8_t 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;
}

static uint8_t 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;
	dsent = 1;
	sdisc = 1;
	sdelay = 1000 + (rand() & 0x1F);
	return ~0;
}

static uint8_t is_bus_free(void)
{

	if(sdelay || slocked || sind)
		return 0;
	return 1;
}

static void announce(void)
{
        sctl = 0xF8;

        saddress = 0xFFFFFFFF;

        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] = 0x30 + ((((uint8_t*)&my_address)[3] >> 4) & 0x0F);
        smessage[15] = 0x30 + (((uint8_t*)&my_address)[3] & 0x0F);
	if(smessage[14] > 0x39) smessage[14] += 7;
	if(smessage[15] > 0x39) smessage[15] += 7;

	if (sendmsg())
		asent = 1;
}

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

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

#ifdef ConfigButton
	DDRD &= ~(1<<ConfigButton);
	PORTD |= (1<<ConfigButton);
#endif

#ifdef ConfigLED
	DDRD |= (1<<ConfigLED);
	PORTD &= ~(1<<ConfigLED);
#endif

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

        OCR2A = 249;
        TIMSK2 |= (1 << OCIE2A);
        TCCR2A |= (1 << WGM21);
        TCCR2B = TCCR2B | (1 << CS22);

	rind = 0;
	sind = 0;
	sdisc = 0;
	rready = 0;
	asent = 0;
	dsent = 0;
	hbw_timer = 0;
	gotack = 0;
	waitack = 0;
	configled = 0;
	configbutton = 0;
	configstate = 0;
	slocked = 0;
	sbusy = 0;
	alink = 0;

	readaddr();
	hbw_read_config();
	srand(my_address);
	sdelay = 1000 + (rand() & 0x1F);
	sei();
}

void snext(void)
{
#ifdef HBW_MAXLINKS
	uint8_t linkchannel;

	if (amessage[0] != 'K') {
		alink = 0;
		return;
	}

	while (alink < HBW_MAXLINKS) {
		linkchannel = eeprom_read_byte((const uint8_t *)(alink*6)+HBW_LINKOFFS);
		if (amessage[1] == linkchannel) {
			((uint8_t*)&aaddress)[0]=eeprom_read_byte((const uint8_t *)(alink*6)+HBW_LINKOFFS+1);
			((uint8_t*)&aaddress)[1]=eeprom_read_byte((const uint8_t *)(alink*6)+HBW_LINKOFFS+2);
			((uint8_t*)&aaddress)[2]=eeprom_read_byte((const uint8_t *)(alink*6)+HBW_LINKOFFS+3);
			((uint8_t*)&aaddress)[3]=eeprom_read_byte((const uint8_t *)(alink*6)+HBW_LINKOFFS+4);
			linkchannel=eeprom_read_byte((const uint8_t *)(alink*6)+HBW_LINKOFFS+5);
			if(aaddress == my_address) {
				hbw_receive_key(aaddress, amessage[1], linkchannel, amessage[3]);
			} else {
				amessage[2] = linkchannel;
				sbusy = 1;
				alink++;
				return;
			}
		}
		alink++;
	}
#endif
	alink = 0;
}

void sprocess(void)
{
	if (gotack) {
		gotack = 0;
		waitack = 0;
		sbusy= 0;
		snext();
		return;
	}

	if (!is_bus_free())
		return;

	if(waitack > 2) {
		waitack = 0;
		sbusy= 0;
		snext();
		return;
	}

	saddress = aaddress;
	sctl = actl;
	slen = alen;
	memcpy(smessage,amessage,slen);

	if (!sendmsg())
		return;

	if (aaddress == 0xFFFFFFFF) {
		sbusy = 0;
		snext();
		return;
	}
	waitack++;
	gotack = 0;
	sdelay = 100 + (rand() & 0x1F);
}

void emessage(void)
{
	uint8_t blocksize;
	uint8_t blocknum;
	uint8_t b,i;

	blocksize = rmessage[3];
	blocknum  = rmessage[4];

	slen = 4 + blocknum / 8;
	if(blocknum % 8) slen++;
	memset(smessage,0,slen);
	smessage[0] = 'e';
	smessage[1] = rmessage[1];;
	smessage[2] = rmessage[2];;
	smessage[3] = rmessage[3];;

	for(b = 0; b <= blocknum; b++) {
		for(i = 0; i < blocksize; i++) {
			if(eeprom_read_byte((const uint8_t *)(b * blocksize + i)) != 0xFF) {
				smessage[4 + b / 8] |= (1 << b % 8);
				break;
			}
		}
	}
}

static void rprocess(void)
{
	uint8_t seq,i;
	uint16_t addr;

	if ((rdest == my_address) &&
	    (raddress == aaddress) &&
	    waitack) {
		waitack = 0;
		gotack = 1;
	}
	if ((rdest == my_address) &&
	    ((rctl & 0x11) == 0x10)) {
		saddress = raddress;
		seq = (rctl >> 1) & 3;
		if(!rlen)
			return;

		switch (rmessage[0]) {
			case '@':
				if (rlen != 6)
					return;
				if (rmessage[1] == 'a') {
					eeprom_write_byte((uint8_t *)EESIZE-4, rmessage[2]);
					eeprom_write_byte((uint8_t *)EESIZE-3, rmessage[3]);
					eeprom_write_byte((uint8_t *)EESIZE-2, rmessage[4]);
					eeprom_write_byte((uint8_t *)EESIZE-1, rmessage[5]);

					readaddr();
				}
				slen = 0;
				sctl = 0x19 | (seq << 5);
				sendmsg();
				break;
			case 'h':
				smessage[0] = HBWTYPE;
				smessage[1] = 0;
				slen = 2;
				sctl = 0x18 | (seq << 5);
				sendmsg();
				break;
			case 'n':
				smessage[0] = HBWSERIAL0;
				smessage[1] = HBWSERIAL1;
				smessage[2] = HBWSERIAL2;
				smessage[3] = HBWSERIAL3;
				smessage[4] = HBWSERIAL4;
				smessage[5] = HBWSERIAL5;
				smessage[6] = HBWSERIAL6;
				smessage[7] = HBWSERIAL7;
			        smessage[8] = 0x30 + ((((uint8_t*)&my_address)[3] >> 4) & 0x0F);
        			smessage[9] = 0x30 + (((uint8_t*)&my_address)[3] & 0x0F);
				if(smessage[8] > 0x39) smessage[8] += 7;
				if(smessage[9] > 0x39) smessage[9] += 7;
                                slen = 10;
                                seq = (rctl >> 1) & 3;
                                sctl = 0x18 | (seq << 5);
                                sendmsg();
				break;
			case 'v':
                                smessage[0] = HBWMAJOR;
                                smessage[1] = HBWMINOR;
                                slen = 2;
                                seq = (rctl >> 1) & 3;
                                sctl = 0x18 | (seq << 5);
                                sendmsg();
				break;
			case 'R':
				if (rlen != 4)
					return;
				if (rmessage[3] > 62)
					return;
				addr = (rmessage[1] >> 8) | rmessage[2];
				for (i = 0; i < rmessage[3]; i++)
					smessage[i] = eeprom_read_byte((const uint8_t *)addr+i);
				slen = rmessage[3];
				seq = (rctl >> 1) & 3;
				sctl = 0x18 | (seq << 5);
				sendmsg();
				break;
			case 'W':
				if (rlen <= 4)
					return;
				if (rlen != rmessage[3] + 4)
					return;
				addr = (rmessage[1] >> 8) | rmessage[2];
				for (i = 0; i < rmessage[3]; i++)
					eeprom_write_byte((uint8_t *)addr+i, rmessage[4+i]);
				slen = 0;
				sctl = 0x19 | (seq << 5);
				sendmsg();
				break;
			case 'x':
			case 's':
				if (rlen <= 2)
					return;
				hbw_set_channel(rmessage[1], rlen - 2, &(rmessage[2]));
			case 'S':
				if (rlen < 2)
					return;
				smessage[0] = 'i';
				smessage[1] = rmessage[1];
				slen = hbw_get_channel(rmessage[1], &(smessage[2])) + 2;
				seq = (rctl >> 1) & 3;
				sctl = 0x18 | (seq << 5);
				sendmsg();
				break;
			case 'C':
				hbw_read_config();
				slen = 0;
				sctl = 0x19 | (seq << 5);
				sendmsg();
				break;
			case 'K':
			case 0xCB:
				if (rlen != 4)
					return;
				hbw_receive_key(raddress, rmessage[1], rmessage[2], rmessage[3]);
				slen = 0;
				sctl = 0x19 | (seq << 5);
				sendmsg();
				break;
			case 'E':
				if (rlen != 4)
					return;
				emessage();
				seq = (rctl >> 1) & 3;
				sctl = 0x18 | (seq << 5);
				sendmsg();
				break;
		}
	}
	if ((rdest == 0xFFFFFFFF) &&
	    ((rctl & 0x11) == 0x10)) {
		if(!rlen)
			return;

		switch (rmessage[0]) {
			case 'z':
				slocked = 1;
				break;
			case 'Z':
				slocked = 0;
				break;
		}
	}
}

static void eeprom_clear(void)
{
	uint16_t i;

	for (i=0; i < EESIZE-4; i++)
		eeprom_write_byte((uint8_t *)i, 0xFF);
}

void hbw_loop(void)
{
	rloop();
	if (rready) {
		rprocess();
		rready = 0;
	}
	if (sbusy) {
		sprocess();
	} else {
		if (!dsent) {
			if (is_bus_free()) {
				senddsc();
			}
		}
		if (!asent) {
			if (is_bus_free()) {
				announce();
			}
		}
	}

#ifdef ConfigButton
	switch (configstate) {
		case 0:
			if (bit_is_clear(PIND, ConfigButton)) {
				configtimer = hbw_timer;
				configstate = 1;
				configled = 1;
			}
			break;
		case 1:
			if (((hbw_timer - configtimer) > 20) &&
			     (bit_is_set(PIND, ConfigButton))) {
				asent = 0;
				configstate = 2;
				configtimer = hbw_timer;
			}
			if ((hbw_timer - configtimer) > 8000) {
				configstate = 3;
				configtimer = hbw_timer;
			}
			break;
		case 2:
			if ((hbw_timer - configtimer) > 2000) {
				configstate = 0;
				configled = 0;
			}
			break;
		case 3:
			configled = (hbw_timer & 128)?0:1;
			if (bit_is_set(PIND, ConfigButton)) {
				configstate = 4;
				configtimer = hbw_timer;
			}
			break;
		case 4:
			configled = (hbw_timer & 128)?0:1;
			if (((hbw_timer - configtimer) > 2000) &&
			     (bit_is_set(PIND, ConfigButton))) {
				configstate = 0;
				configled = 0;
			}
			if (((hbw_timer - configtimer) > 20) &&
			     (bit_is_clear(PIND, ConfigButton))) {
				configstate = 5;
				configled = 0;
			}
			break;
		case 5:
			configled = (hbw_timer & 512)?0:1;
			if (((hbw_timer - configtimer) > 20) &&
			     (bit_is_set(PIND, ConfigButton))) {
				eeprom_clear();
				configstate = 0;
				configled = 0;
				asent = 0;
			}
	}
#endif

#ifdef ConfigLED
	if (configled)
		PORTD |= (1<<ConfigLED);
	else
		PORTD &= ~(1<<ConfigLED);
#endif
}

uint8_t hbw_send_key(uint8_t channel, uint8_t count, uint8_t flag)
{
	if (sbusy)
		return 0;

	if (alink)
		return 0;

	alen = 4;
	actl = 0xF8;
	amessage[0] = 'K';
	amessage[1] = channel;
	amessage[2] = 0;
	amessage[3] = (flag & 3) | (count << 2);
		
	aaddress = 0xFFFFFFFF;
	sbusy = 1;
	alink = 0;
	return 1;
}

uint8_t hbw_send_channel(uint8_t channel, uint8_t length, uint8_t const * const data)
{
	if (sbusy)
		return 0;

	if (alink)
		return 0;

	alen = length + 2;
	actl = 0xF8;
	((uint8_t*)&aaddress)[0]=eeprom_read_byte((const uint8_t *)2);
	((uint8_t*)&aaddress)[1]=eeprom_read_byte((const uint8_t *)3);
	((uint8_t*)&aaddress)[2]=eeprom_read_byte((const uint8_t *)4);
	((uint8_t*)&aaddress)[3]=eeprom_read_byte((const uint8_t *)5);

	amessage[0] = 'i';
	amessage[1] = channel;
	memcpy(&(amessage[2]), data, length);
	sbusy = 1;
	return 1;
}

ISR(USART_RX_vect)
{
	uint8_t c;

	sdelay = 100 + (rand() & 0x1F);

	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) {
		((uint8_t*)&rdest)[rind-2] = c;
		return;
	}
	if (rind == 6) {
		rctl = c;
		return;
	}
	if (rind < 11) {
		((uint8_t*)&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;
		rlen = rlen - 2;
		return;
	}
}


ISR(TIMER2_COMPA_vect)
{
	cli();
	hbw_timer++;
	if (sdelay)
		sdelay--;
	sei();
}