int edasi = 0;
int tagasi = 0;
int toota=1;
int a=0;
/* pesa asjad */
int viimane_dekaad=0;/* viimati olime pesas sellel dekaadil */
int soida_pessa=0;
int cur_dekaad=0; 
#define DEKAADE 3


#define TUNDLIKUS 190

void serial_putchar(int c)
{
    while (!(peek(0x102e) & 0x80));/* wait until serial transmit empty */
    poke(0x102f, c);  /* send character */
}
void disable_pcode_serial()   /* necessary to receive characters using serial_getchar */
{
   poke(0x3c, 1);
}

void reenable_pcode_serial()   /* necessary for IC to interact with board again */
{
   poke(0x3c, 0);
}

int serial_getchar()
{
   if (peek(0x102e) & 0x20) /* wait for received character */
   return peek(0x102f);
   else
	return 255; 
}

/*Masinad seisma*/
void stop(){
    clear_digital_out(1);
    clear_digital_out(3);
}
void mine_edasi(){
    set_digital_out(0);
    clear_digital_out(2);
    set_digital_out(1);
    set_digital_out(3);
    printf("Edasi\n");
}

void poora(int number) {
    if(number == 255) 
		return;
    if ((number > 1) && (number < 28) ) {
        number = 31 - number;
        clear_digital_out(0);
        clear_digital_out(2);
    }
    else {
        if((number > 36) && (number < 64)) {
            number = -31 + number;
            set_digital_out(0);
            set_digital_out(2);
        }
    }
    set_digital_out(1);
    set_digital_out(3);
   
    sleep(0.00036 * (float)number);
/*    sleep(0.036 * (float)number);*/
    clear_digital_out(2);
    set_digital_out(0);
}

void poora_90(int suund, int aeg){
    /* suund =1 siis prame pripeva*/
    if(suund==1){
        set_digital_out(0);
        set_digital_out(2);
        printf("Paremale %d\n",suund);


    }
    if (suund==2){
        clear_digital_out(0);
        clear_digital_out(2);
        printf("Vasakule %d\n",suund);

    }
    set_digital_out(1);
    set_digital_out(3);
    sleep((float)aeg);
    set_digital_out(0);
    clear_digital_out(2);
}
void pesa_leitud() {
	bk(1);
	set_digital_out(2);
	clear_digital_out(0);
	set_digital_out(1);
	set_digital_out(3);
	sleep(2.0);
	off(1);
	poora_90(1,2);
	clear_digital_out(1);
	clear_digital_out(3);
	stop();
}
void poora_uherattaga(int suund){
    /* suund =1 siis prame pripeva*/
    if(suund==1){
        set_digital_out(0);
        set_digital_out(2);
        printf("Paremale %d\n",suund);
	  set_digital_out(1);
	  clear_digital_out(3);
    }
    if (suund==2){
        clear_digital_out(0);
        clear_digital_out(2);
        printf("Vasakule %d\n",suund);
        clear_digital_out(1);
        set_digital_out(3);
    }
    sleep(0.2);
    set_digital_out(0);
    clear_digital_out(2);
}

void servo(){
int i;

init_expbd_servos(1);
servo0=0;
for (i=0;i<10;i+=1) {
	servo0=0;
	sleep(1.0);
	servo0=1600;
	sleep(1.0);	}
}




void main(){
reenable_pcode_serial();

    while(!stop_button()) {
      printf("%d %d\n", analog(4), analog(6));
    }
    while (!start_button()) { 
	disable_pcode_serial();
		fd(3);     
	  if ((cur_dekaad-viimane_dekaad)>=DEKAADE) soida_pessa=1;
        if (!(digital(7) || digital(8)) && (analog(4)<TUNDLIKUS) && (analog(6)<TUNDLIKUS) ) {
            a = serial_getchar();
		if( ((a-192) >= 1 ) && (a!=255) ) {
			cur_dekaad=a-192;
		}else{
            	if((soida_pessa)&& ((a>64) && (a<128) ))
				a=a-64;
			if (a){
				printf("%d\n",a);
            		poora(a);
			}
			
			
		}
        }
	  else {	
		if(digital(7)) {
                poora_90(1,1);
            }
            if(digital(8)) {
                poora_90(2,1);
            }
		if((analog(4)>TUNDLIKUS) || (analog(6)>TUNDLIKUS) ) {
			stop();
			sleep(0.5);
			if((analog(4)>TUNDLIKUS) && (analog(6) < TUNDLIKUS)) {
			    poora_uherattaga(2);
			}
			else if((analog(4)<TUNDLIKUS) && (analog(6) > TUNDLIKUS)) {
      	 	    poora_uherattaga(1);
			}
			else if((analog(4)>TUNDLIKUS) && (analog(6) > TUNDLIKUS)) {
					bk(3);		   
				 pesa_leitud();
				 soida_pessa=0;
				 viimane_dekaad=cur_dekaad;
			}
			mine_edasi();
		}
        }
    }
}