//#define DEBUG

//#define DEBUG2

#include "defines.h"

#include <ctype.h>
#include <stdint.h>
#include <stdio.h>
#include <string.h>

#include <avr/interrupt.h>
//#include <avr/pgmspace.h>

#include <util/delay.h>

#include "uart.h"
#include "i2c.h"

static uint16_t tmr;

static int types[24];
static int value[24];
static int pin[24];
static int prt[24];

static int8_t speed1s[20];
static int8_t speed1f[20];
static int8_t speed2s[20];
static int8_t speed2f[20];
static uint16_t time[20];
static int queue_size;
static uint8_t current_timer;

static int adc_id;
static int adc_conv;

#define WORKSPACE 100

#define MAXSERVO 0x53
#define MINSERVO 0x0D

SIGNAL(SIG_OVERFLOW0){
  TCNT0=0x6C;

  if(tmr<WORKSPACE*8){

		  if(tmr<WORKSPACE*4){

		      if(tmr<WORKSPACE*2){

			      if(tmr<WORKSPACE){
		    
					uint8_t tmp=tmr;

				    if(types[0]==2){
					  if(tmp==0){ outb(PORTA,inb(PORTA) | 0x01); }; //Set PA0 pin to 1
				      uint16_t xxx=value[0];
					  if(xxx>MAXSERVO){ xxx=MAXSERVO; };
					  if(xxx<MINSERVO){ xxx=MINSERVO; };
				      if(tmp==xxx){ outb(PORTA,inb(PORTA) & ~0x01); }; //Set PA0 pin to 0
				    };
				    if(types[1]==2){
					  if(tmp==0){ outb(PORTA,inb(PORTA) | 0x02); }; //Set PA1 pin to 1
				      uint8_t xxx=value[1];
					  if(xxx>MAXSERVO){ xxx=MAXSERVO; };
					  if(xxx<MINSERVO){ xxx=MINSERVO; };
				      if(tmp==xxx){ outb(PORTA,inb(PORTA) & ~0x02); }; //Set PA1 pin to 0
				    };
					if(types[2]==2){
					  if(tmp==0){ outb(PORTA,inb(PORTA) | 0x04); }; //Set PA2 pin to 1
				      uint8_t xxx=value[2];
					  if(xxx>MAXSERVO){ xxx=MAXSERVO; };
					  if(xxx<MINSERVO){ xxx=MINSERVO; };
				      if(tmp==xxx){ outb(PORTA,inb(PORTA) & ~0x04); }; //Set PA2 pin to 0
				    };

				  }else{

				    uint8_t tmp=tmr-WORKSPACE;
		  

					if(types[3]==2){
					  if(tmp==0){ outb(PORTA,inb(PORTA) | 0x08); }; //Set PA3 pin to 1
				      uint8_t xxx=value[3];
					  if(xxx>MAXSERVO){ xxx=MAXSERVO; };
					  if(xxx<MINSERVO){ xxx=MINSERVO; };
				      if(tmp==xxx){ outb(PORTA,inb(PORTA) & ~0x08); }; //Set PA3 pin to 0
				    };
					if(types[4]==2){
					  if(tmp==0){ outb(PORTA,inb(PORTA) | 0x10); }; //Set PA4 pin to 1
				      uint8_t xxx=value[4];
					  if(xxx>MAXSERVO){ xxx=MAXSERVO; };
					  if(xxx<MINSERVO){ xxx=MINSERVO; };
				      if(tmp==xxx){ outb(PORTA,inb(PORTA) & ~0x10); }; //Set PA4 pin to 0
				    };
					if(types[5]==2){
					  if(tmp==0){ outb(PORTA,inb(PORTA) | 0x20); }; //Set PA5 pin to 1
				      uint8_t xxx=value[5];
					  if(xxx>MAXSERVO){ xxx=MAXSERVO; };
					  if(xxx<MINSERVO){ xxx=MINSERVO; };
				      if(tmp==xxx){ outb(PORTA,inb(PORTA) & ~0x20); }; //Set PA5 pin to 0
				    };

				  };
		      }else{

				  if(tmr<WORKSPACE*3){

				    uint8_t tmp=tmr-WORKSPACE*2;

					if(types[6]==2){
					  if(tmp==0){ outb(PORTA,inb(PORTA) | 0x40); }; //Set PA6 pin to 1
				      uint8_t xxx=value[6];
					  if(xxx>MAXSERVO){ xxx=MAXSERVO; };
					  if(xxx<MINSERVO){ xxx=MINSERVO; };
				      if(tmp==xxx){ outb(PORTA,inb(PORTA) & ~0x40); }; //Set PA6 pin to 0
				    };
					if(types[7]==2){
					  if(tmp==0){ outb(PORTA,inb(PORTA) | 0x80); }; //Set PA7 pin to 1
				      uint8_t xxx=value[7];
					  if(xxx>MAXSERVO){ xxx=MAXSERVO; };
					  if(xxx<MINSERVO){ xxx=MINSERVO; };
				      if(tmp==xxx){ outb(PORTA,inb(PORTA) & ~0x80); }; //Set PA7 pin to 0
				    };
					if(types[8]==2){
					  if(tmp==0){ outb(PORTC,inb(PORTC) | 0x80); }; //Set PC7 pin to 1
				      uint8_t xxx=value[8];
					  if(xxx>MAXSERVO){ xxx=MAXSERVO; };
					  if(xxx<MINSERVO){ xxx=MINSERVO; };
				      if(tmp==xxx){ outb(PORTC,inb(PORTC) & ~0x80); }; //Set PC7 pin to 0
				    };

				  }else{
		  
					uint8_t tmp=tmr-WORKSPACE*3;

				    if(types[9]==2){
					  if(tmp==0){ outb(PORTC,inb(PORTC) | 0x40); }; //Set PC6 pin to 1
				      uint8_t xxx=value[9];
					  if(xxx>MAXSERVO){ xxx=MAXSERVO; };
					  if(xxx<MINSERVO){ xxx=MINSERVO; };
				      if(tmp==xxx){ outb(PORTC,inb(PORTC) & ~0x40); }; //Set PC6 pin to 0
				    };
					if(types[10]==2){
					  if(tmp==0){ outb(PORTC,inb(PORTC) | 0x20); }; //Set PC5 pin to 1
				      uint8_t xxx=value[10];
					  if(xxx>MAXSERVO){ xxx=MAXSERVO; };
					  if(xxx<MINSERVO){ xxx=MINSERVO; };
				      if(tmp==xxx){ outb(PORTC,inb(PORTC) & ~0x20); }; //Set PC5 pin to 0
				    };
					if(types[11]==2){
					  if(tmp==0){ outb(PORTC,inb(PORTC) | 0x10); }; //Set PC4 pin to 1
				      uint8_t xxx=value[11];
					  if(xxx>MAXSERVO){ xxx=MAXSERVO; };
					  if(xxx<MINSERVO){ xxx=MINSERVO; };
				      if(tmp==xxx){ outb(PORTC,inb(PORTC) & ~0x10); }; //Set PC4 pin to 0
				    };
					

				  };
		      };
		  }else{
		      if(tmr<WORKSPACE*6){

				  if(tmr<WORKSPACE*5){

				    uint8_t tmp=tmr-WORKSPACE*4;

					if(types[12]==2){
					  if(tmp==0){ outb(PORTB,inb(PORTB) | 0x80); }; //Set PB3 pin to 1
				      uint8_t xxx=value[12];
					  if(xxx>MAXSERVO){ xxx=MAXSERVO; };
					  if(xxx<MINSERVO){ xxx=MINSERVO; };
				      if(tmp==xxx){ outb(PORTB,inb(PORTB) & ~0x80); }; //Set PB3 pin to 0
				    };
				    if(types[13]==2){
					  if(tmp==0){ outb(PORTB,inb(PORTB) | 0x40); }; //Set PB2 pin to 1
				      uint8_t xxx=value[13];
					  if(xxx>MAXSERVO){ xxx=MAXSERVO; };
					  if(xxx<MINSERVO){ xxx=MINSERVO; };
				      if(tmp==xxx){ outb(PORTB,inb(PORTB) & ~0x40); }; //Set PB2 pin to 0
				    };
					if(types[14]==2){
					  if(tmp==0){ outb(PORTD,inb(PORTD) | 0x20); }; //Set PD5 pin to 1
				      uint8_t xxx=value[14];
					  if(xxx>MAXSERVO){ xxx=MAXSERVO; };
					  if(xxx<MINSERVO){ xxx=MINSERVO; };
				      if(tmp==xxx){ outb(PORTD,inb(PORTD) & ~0x20); }; //Set PD5 pin to 0
				    };

				  }else{

				    uint8_t tmp=tmr-WORKSPACE*5;
		    
					if(types[15]==2){
					  if(tmp==0){ outb(PORTD,inb(PORTD) | 0x10); }; //Set PD4 pin to 1
				      uint8_t xxx=value[15];
					  if(xxx>MAXSERVO){ xxx=MAXSERVO; };
					  if(xxx<MINSERVO){ xxx=MINSERVO; };
				      if(tmp==xxx){ outb(PORTD,inb(PORTD) & ~0x10); }; //Set PD4 pin to 0
				    };
				  };
			  };

		  };

  }else{

     if(tmr==800){
		
		//Do motor control 50 times per second
		
		if( queue_size>0 ){

		  if( current_timer == time[0] ){
		    queue_size--;
			int i;
			for(i=0; i<queue_size; i++){
			  speed1s[i]=speed1s[i+1];
			  speed1f[i]=speed1f[i+1];
			  speed2s[i]=speed2s[i+1];
			  speed2f[i]=speed2f[i+1];
			  time[i]=time[i+1];
			};
			current_timer=0;
		  };

		  //printf("+");

		  if( queue_size>0 ){

			int16_t d;
			int32_t x;

		    d=speed2f[0]; d-=speed2s[0];
			x=current_timer; x*=d; x/=time[0]; x+=speed2s[0];

			uint8_t speed;
			if(x>0){
                outb(PORTB,inb(PORTB) & ~0x02); //Set PB1=0 Drive1 - Forward
				speed=x;
				speed+=0x80;
			}else if(x<0){
                outb(PORTB,inb(PORTB) |  0x02); //Set PB1=1 Drive1 - Backward
				speed=-x;
				speed+=0x7F;
			}else{
                outb(PORTB,inb(PORTB) & ~0x02); //Set PB1=0 Drive1 - Stop(forward)
				speed=0;
			};
			OCR1BH=0x00;
      		OCR1BL=speed;


		    d=speed1f[0]; d-=speed1s[0];
			x=current_timer; x*=d; x/=time[0]; x+=speed1s[0];
			if(x>0){
				outb(PORTB,inb(PORTB) & ~0x01); //Set PB0=0 Drive0 - Forward
				uint8_t speed=x;
				speed+=0x80;
			}else if(x<0){
				outb(PORTB,inb(PORTB) |  0x01); //Set PB0=1 Drive0 - Backward
				uint8_t speed=-x;
				speed+=0x7F;
			}else{
				outb(PORTB,inb(PORTB) & ~0x01); //Set PB0=0 Drive0 - Stop(forward)
				uint8_t speed=0;
			};
			OCR1AH=0x00;
      		OCR1AL=speed;


			current_timer++;
		  }else{
		    OCR1BH=0x00;
      		OCR1BL=0x00;
			OCR1AH=0x00;
      		OCR1AL=0x00;
		  };

		};

	 }else{

	     //Do ADC scan in 801-2000 cycles

	     if(tmr==801){
		 	adc_id=0;
			while( types[adc_id]!=3 && adc_id<8) adc_id++;
			adc_conv=0;
		 };

	 
		 if(adc_id<8){
		    if(adc_conv==0){
		   		outb(ADMUX, (inb(ADMUX) & ~0x1F) | (adc_id & 0x1F));	// set channel
				sbi(ADCSRA, ADIF);						// clear hardware "conversion complete" flag 
				sbi(ADCSRA, ADSC);						// start conversion
				adc_conv=1;
			}else{
				if( bit_is_set(ADCSRA, ADSC) ){		// wait until conversion complete
					adc_conv=0;
					if(types[adc_id]==3) value[adc_id]=inb(ADCH);
					while( types[adc_id]!=3 && adc_id<8) adc_id++;
				};
			};

		 };

	 };
	 

  };

  tmr++;
  if(tmr==2000){
    tmr=0;
  };

};

static void ioinit(void)
{
  uart_init();
  i2cInit();
};

static void dec2hex(int x, char *c1, char *c2){
  char cc1=(x & 0xf0)>>4;
  char cc2=(x & 0x0f);
  *c1=cc1>9?cc1+'A'-10:cc1+'0';
  *c2=cc2>9?cc2+'A'-10:cc2+'0';
};

static int hex2dec(char a, char b, uint8_t* res){
  if(a<'0' || a>'F' || (a>'9' && a<'A')){
	return 0;
  };
  if(b<'0' || b>'F' || (b>'9' && b<'A')){
	return 0;
  };
  *res=(a>'9'?a-'A'+10:a-'0')*16 + (b>'9'?b-'A'+10:b-'0');

  #ifdef DEBUG
  printf("hex2dec(%c%c)=%d\n",a,b,*res);
  #endif

  return 1;
};

//static void requestI2Cbus(int adr, char* request, int req_len, char* answer, int ans_len){
//  i2cMasterTransferNI(adr << 1, req_len, request, ans_len, answer);
//};

static int requestI2Cbus2(int adr, char* request, int req_len, char* answer, int ans_len){
  return i2cMasterTransferNI2(adr << 1, req_len, request, ans_len, answer, 1000);
};

static void update_t(int i){
  uint8_t snd;

  #ifdef DEBUG
  printf("Ut[%d,%d,%d]\n",i,types[i],prt[i]);
  #endif

  if( prt[i]==0 ){ snd=inb(DDRA); };
  if( prt[i]==1 ){ snd=inb(DDRB); };
  if( prt[i]==2 ){ snd=inb(DDRC); };
  if( prt[i]==3 ){ snd=inb(DDRD); };
  //if(types[i]==0 || types[i]==3){ snd=snd & (0xff-pin[i]); }; //TTL input pin
  if(types[i]==0){ snd=snd & (0xff-pin[i]); }; //TTL input pin
  if(types[i]==3){ snd=snd & (0xff-pin[i]); }; //TTL input pin
  if(types[i]==1 || types[i]==2){ snd=snd | pin[i]; }; //TTL output pin 
  if( prt[i]==0 ){ outb(DDRA,snd); };
  if( prt[i]==1 ){ outb(DDRB,snd); };
  if( prt[i]==2 ){ outb(DDRC,snd); };
  if( prt[i]==3 ){ outb(DDRD,snd); };
};

static void update_r(int i){
  if( types[i]==0) //TTL input pin
  {

	  uint8_t snd;
	  if( prt[i]==0 ){ snd=inb(PINA); };
	  if( prt[i]==1 ){ snd=inb(PINB); };
	  if( prt[i]==2 ){ snd=inb(PINC); };
	  if( prt[i]==3 ){ snd=inb(PIND); };

	  #ifdef DEBUG
	  printf("Ur[%d,%d,%d,%d]=%d\n",i,types[i],prt[i],value[i],snd);
	  #endif

	  value[i]=(snd & pin[i])?1:0;
	  return;
  };
};

static void update_w(int i){

  uint8_t snd;

  #ifdef DEBUG
  printf("Uw[%d,%d,%d,%d]\n",i,types[i],prt[i],value[i]);
  #endif

  if( i>= 0x10 ){
    if( i==0x10 ){
	  if( value[i]==02 ){
		outb(PORTB,inb(PORTB) |  0x01); //Set PB0=1
      }else{
	    outb(PORTB,inb(PORTB) & ~0x01); //Set PB0=0
	  };
      return;
	};
	if( i==0x11 ){
	  printf("PWM0=%d\n",value[i]);
	  OCR1AH=0x00;
      OCR1AL=value[i];
	};
    if( i==0x12 ){
	  if( value[i]==02 ){
		outb(PORTB,inb(PORTB) |  0x02); //Set PB1=1
      }else{
	    outb(PORTB,inb(PORTB) & ~0x02); //Set PB1=0
	  };
	};
	if( i==0x13 ){
	  printf("PWM1=%d\n",value[i]);
	  OCR1BH=0x00;
      OCR1BL=value[i];
	};
	return;
  };

  if( prt[i]==0 ){ snd=inb(PORTA); };
  if( prt[i]==1 ){ snd=inb(PORTB); };
  if( prt[i]==2 ){ snd=inb(PORTC); };
  if( prt[i]==3 ){ snd=inb(PORTD); };

  if( (types[i]==0 || types[i]==1) ) //TTL i/o pin
  {
     if(value[i]==0){
	   snd=snd & (0xff - pin[i]);
	 }else{
	   snd=snd | pin[i];
	 };
  };

  if( prt[i]==0 ){ outb(PORTA,snd); };
  if( prt[i]==1 ){ outb(PORTB,snd); };
  if( prt[i]==2 ){ outb(PORTC,snd); };
  if( prt[i]==3 ){ outb(PORTD,snd); };
};

static int processI2Crequest(int adr, char* request, int req_len, char* answer){
  
  printf("%d ",request[0]);
  printf("req_len=%d\n",req_len);
  
  if(adr!=255){ return 0; };
  if(req_len==1){
    if(request[0]==255){
	  //Version info
      answer[0]=0x00;
      answer[1]=0x00;
	  answer[2]=0x00;
	  answer[3]=0x01;
	  return 1;
    };
  };

  if(request[0]==0x14){
    if(req_len==1){
	  queue_size=0;
	  printf("@");
	  return 1;
	}else{
	  if( queue_size==0 ){ current_timer=0; };
	  int r6=0;
	  while(r6+4<req_len){
	    speed1s[queue_size]=request[2+r6];
	    speed2s[queue_size]=request[3+r6];
	    speed1f[queue_size]=request[4+r6];
	    speed2f[queue_size]=request[5+r6];
	    time[queue_size]=request[1+r6];
	    queue_size++;
		r6+=5;
	  };
	  printf("!");
	  return 1;
	};
  };
  
  if(req_len==3 || req_len==2){
    if(request[0]<0x14){

	  #ifdef DEBUG
	  printf("Set value[%d]=%d\n",request[0],request[1]);
	  #endif
	  
	  value[request[0]]=request[1];
	  update_w(request[0]);
	  return 1;
	};
	if(request[0]>=0x20 && request[0]<0x30){

	  #ifdef DEBUG
	  printf("Set types[%d]=%d\n",request[0]-0x20,request[1]);
	  #endif

  	  types[request[0]-0x20]=request[1];
	  update_t(request[0]-0x20);
	  return 1;
	};
  };
  if(req_len==1){
    if(request[0]>=0x80 && request[0]<0x90){
	  update_r(request[0]-0x80);
  	  answer[0]=value[request[0]-0x80];
	  
	  #ifdef DEBUG
	  printf("Get values[%d]=%d\n",request[0]-0x80,answer[0]);
	  #endif

	  return 1;
	};
	if(request[0]>=0xA0 && request[0]<0xB0){
  	  answer[0]=types[request[0]-0xA0];

	  #ifdef DEBUG
	  printf("Get types[%d]=%d\n",request[0]-0xA0,answer[0]);
	  #endif

	  return 1;
	};
  };
  return 1;
};

static int process(char* buf, uint8_t len){
  //Getting i2c request from RS-232 request
  if(len<5){
    printf("X01Command\n");
	return 0;
  };
  if(buf[0]!='Q'){
    printf("X02Command\n");
	return 0;
  };
  uint8_t adr;
  if( !hex2dec(buf[1],buf[2],&adr) ){
    printf("X03Address\n");
	return 0;
  };
  uint8_t readBytes=0;
  if( !hex2dec(buf[3],buf[4],&readBytes) ){
    printf("X04NumBytesToRead\n");
	return 0;
  };
  if( readBytes>64 ){
    printf("X05NumBytesToRead\n");
    return 0;
  };
  int current=5;
  if( ((len-current) & 0x01) == 1){
    printf("X06BytesToSend\n");
	return 0;
  };
  uint8_t bytebuf1[64];
  uint8_t bytebuf2[64];
  int sendBytes=0;
  while(current<len){
    if( !hex2dec(buf[current],buf[current+1],&(bytebuf1[sendBytes])) ){
      printf("X07BytesToSend\n");
	  return 0;
    };
	current+=2;
	sendBytes++;
	if( sendBytes>64 ){
      printf("X08NumBytesToSend\n");
	  return 0;
    };
  };

  //Processing i2c request
  #ifdef DEBUG2
  printf("Address: %d\n",adr);
  printf("Bytes To Write: %d\n",sendBytes);
  printf("Bytes To Read: %d\n",readBytes);
  #endif

  //Test bytesbuf[] clearing
  int j=0;
  while(j<readBytes){
    bytebuf2[j]=0;
    j++;
  };
  
  int res;
  if(adr>127){
    res=processI2Crequest(adr,bytebuf1,sendBytes,bytebuf2);
  }else{
    res=requestI2Cbus2(adr,bytebuf1,sendBytes,bytebuf2,readBytes);
  };
  
  if(res==0){
    printf("X00Timeout!\n");
	return 0;
  };

  //Forming answer from i2c answer for RS-232
  //printf("%c%c%c",buf[0],buf[1],buf[2]);
  printf("R");
  current=3;
  int i=0;
  while(i<readBytes){
    char c1,c2;
    dec2hex(bytebuf2[i],&c1,&c2);
    printf("%c%c",c1,c2);
	i++;
  };
  printf("\n");
  return 1;
  
};

FILE uart_str = FDEV_SETUP_STREAM(uart_putchar, uart_getchar, _FDEV_SETUP_RW);

int main(void)
{

  char buf[100];

  pin[0]=0; prt[0]=0;
  pin[1]=1; prt[1]=0;
  pin[2]=2; prt[2]=0;
  pin[3]=3; prt[3]=0;
  pin[4]=4; prt[4]=0;
  pin[5]=5; prt[5]=0;
  pin[6]=6; prt[6]=0;
  pin[7]=7; prt[7]=0;
  pin[8]=7; prt[8]=2;
  pin[9]=6; prt[9]=2;
  pin[10]=5; prt[10]=2;
  pin[11]=4; prt[11]=2;
  pin[12]=3; prt[12]=1;
  pin[13]=2; prt[13]=1;
  pin[14]=5; prt[14]=3;
  pin[15]=4; prt[15]=3;

  ioinit();

  stdout = stdin = &uart_str;
  
  int i;
  for(i=0; i<16; i++){
    pin[i]=1 << pin[i];
    types[i]=0;
	update_t(i);
	value[i]=0;
	update_w(i);
  };

  for(i=16; i<22; i++){
    pin[i]=1 << pin[i];
	value[i]=0;
	update_w(i);
    types[i]=1;
	update_t(i);
  };

  sbi(ADCSRA, ADEN);									// enable ADC (turn on ADC power)
  cbi(ADCSRA, ADATE);									// default to single sample convert mode
  outb(ADCSRA, ((inb(ADCSRA) & ~0x07) | 0x06));		// set default prescaler CPU clk/64
  outb(ADMUX, ((inb(ADMUX) & ~0xC0) | (0x01<<6)));	// set default reference VCC pin
  sbi(ADMUX, ADLAR);								// set to left-adjusted result
  

  // Initiating Timer/Counter-1 Control Registers:
  //  
  // TCCR1A = COM1A1:COM1A0:COM1B1:COM1B0:FOC1A:FOC1B:WGM11:WGM10
  //             1      0      1      0     0     0     0     1
  //
  // TCCR1B = ICNC1:ICES1:-:WGM13:WGM12:CS12:CS11:CS10
  //            0     0   0   0     1     0    1    0
  //
  // COM1x1:COM1x0 = 1:0 => OC1x connected to output pin, reset on compare
  // FOC1A:FOC1B = 0:0 => Don't force output compare event
  // WGM13:WGM12:WGM11:WGM10 = 0:1:0:1 => Fast PWM 8-bit
  // ICNC1 = 0 => Input Capture Noise Canceller disabled (because not needed)
  // ICES1 = 0 => Input Capture Edge Select set to "falling" mode (no matter)
  // CS12:CS11:CS10 = 0:1:0 => Timer "on", 1/8 prescaling (1 clock every 8 cpu tact)
  TCCR1A=0xA1;
  TCCR1B=0x0A;

  TCNT1H=0x00;
  TCNT1L=0x00;

  // Initiating Timer/Counter-0 Control Registers:
  //  
  // TCCR0 = FOC0:WGM00:COM01:COM00:WGM01:CS02:CS01:CS00
  //          0     0     0     0     0     0    0    1
  //
  // COM1x1:COM1x0 = 0:0 => OC1x disconnected from output pin
  // FOC1A:FOC1B = 0:0 => Don't force output compare event
  // WGM13:WGM12:WGM11:WGM10 = 0:0:0:0 => Normal timer/counter operation
  // ICNC1 = 0 => Input Capture Noise Canceller disabled (because not needed)
  // ICES1 = 0 => Input Capture Edge Select set to "falling" mode (no matter)
  // CS12:CS11:CS10 = 0:0:1 => Timer "on", no prescaling (1 clock every cpu tact)
  // TCCR1A = 00000000 = 0x00
  TCCR0=0x01;

  TCNT0=0x6C;

 
  // Initiating internal program subcounter with zero
  tmr=0;

  // Initiating Timer/Counter Interrupt Mask register:
  //
  // TIMSK = OCIE2:TOIE2:TICIE1:OCIE1A:OCIE1B:TOIE1:OCIE0:TOIE0
  //           0     0      0      0      0     1     0     0
  //
  // OCIE2 = 0 => Timer/Counter2 Output Compare Match Interrupt disabled
  // TOIE2 = 0 => Timer/Counter2 Timer Overflow disabled
  // TICIE1 = 0 => Timer/Counter1 Input Capture Interrupt disabled
  // OCIE1x = 0 => Timer/Counter1 Output Compare Match Interrupt disabled
  // TOIE1 = 0 => Timer/Counter1 Timer Overflow enabled
  // OCIE0 = 0 => Timer/Counter0 Output Compare Match Interrupt disabled
  // TOIE0 = 1 => Timer/Counter0 Timer Overflow disabled
  TIMSK=0x01;

  // Initiating Input Capture Register for unknown reason (?)
  ICR1H=0x00;
  ICR1L=0x00;

  // Initiating Output Compare Registers for unknown reason (?)
  OCR1AH=0x00;
  OCR1AL=0x00;
  OCR1BH=0x00;
  OCR1BL=0x00;

  // Initiating Output Compare Registers for unknown reason (?)
  OCR0=0x00;

  queue_size=0;

  DDRB |= 0x03; //Set DIR0 & DIR1 (PB0 & BP1) ports as output
  DDRD |= 0x30; //Set PWM0 & PWM1 (PD4 & PD5) ports as output

  sei();

  printf("Ready!\n");

  while(1){
    if (fgets(buf, sizeof buf - 1, stdin) == NULL) break;
	int l=strlen(buf)-1;
	printf("Len:%d\n",l),
	printf("<%s>",buf);

	#ifdef DEBUG2
	printf("Len:%d\n",l),
	#endif

    process(buf,l);
  };
 
  return 0;
  
}
