#include "../../lcd.h"
typedef unsigned char byte;
typedef signed char sbyte;
typedef unsigned short word;

struct Ports {
	byte porta;
	byte reserved1;
	byte pioc;
	byte portc;
	byte portb;
	byte portcl;
	byte reserved2;
	byte ddrc;
	byte portd;
	byte ddrd;
	byte porte;
};

struct Timer {
	byte cforc;
	byte oc1m;
	byte oc1d;
	word tcnt;
	word tic1;
	word tic2;
	word tic3;
	word toc1;
	word toc2;
	word toc3;
	word toc4;
	word toc5;
	byte tctl1;
	byte tctl2;
	byte tmsk1;
	byte tflg1;
	byte tmsk2;
	byte tflg2;
};

void oc1_isr(void) __attribute__((interrupt));

typedef void (*isr)(void);
#define E (isr)0xffff
extern void _start(void);

isr vectors[32] __attribute__ ((section (".vectors"))) = {
	E, E, E, E, E, E, E, E,
	E, E, E, E, E, E, E, E,
	E, E, E, E, oc1_isr, E, E, E,
	E, E, E, E, E, E, E, _start
};

volatile byte snelheid=0;
volatile byte rechts=0;

void oc1_isr(void) {
	static byte patroon[]={0x48, 0x50, 0x30, 0x28};
	volatile struct Timer* timer=(volatile struct Timer*)0x100b;
	static sbyte i=0;
	timer->tflg1=0x80;
	if (snelheid==0) { 
		timer->toc1+=0x1000;
	}
	else {
		timer->toc1+=10000/snelheid;
		timer->oc1d=patroon[i];
		if (rechts) {
			if (++i==4)
				i=0;
		}
		else {
			if (--i==-1)
				i=3;
		}
	}	
}

void display(void) {
	lcd_setpos(0,0);
	lcd_puts("Snelheid = ");
	lcd_putc('0'+snelheid/10);
	lcd_putc('0'+snelheid%10);
	lcd_setpos(2,0);
	lcd_puts("Richting = ");
	if (rechts)
		lcd_puts("rechts");
	else
		lcd_puts("links ");
}		

int main() {
	volatile struct Ports* ports=(volatile struct Ports*)0x1000;
	volatile struct Timer* timer=(volatile struct Timer*)0x100b;
	lcd_init();
	display();
	timer->oc1m=0x78;
	timer->oc1d=0;
	timer->toc1=timer->tcnt+50;
	timer->tmsk1=0x80;
	while (1) {
		byte oude_snelheid=snelheid;
		byte oude_rechts=rechts;
		snelheid = ports->portc&0x0f;
		if (snelheid<3) // richting alleen omdraaien bij snelheid < 3
			rechts = (ports->portc&0x10)==0x10;
		if (rechts!=oude_rechts||snelheid!=oude_snelheid)
			display();		
	}
	return 0;		
}