#define ROBOTOOLS_INCLUDED 1 
#include "robotools.c" 
#include "graph.c" 

struct pont curr_poz;
struct csucs *curr_csucs, *start_csucs;
int curr_dir;
struct csucslista* csfej;
char csucs_dump_pointer[250];
int csucs_festes=1;
// a hack
int first_junction=0;

void unknown_csucs(char* what) {
  if (what[3]== '2' && first_junction++) {
    curr_poz=pont_move(curr_poz, curr_dir);
    curr_csucs=csucs_move(curr_csucs, curr_dir);
    known_csucs(curr_csucs, "010", curr_dir, curr_poz, csfej, 0);
    elfest(curr_csucs, curr_dir);
    curr_poz=pont_move(curr_poz, curr_dir);
    curr_csucs=csucs_move(curr_csucs, curr_dir);
    known_csucs(curr_csucs, what, curr_dir, curr_poz, csfej, 1);
  } else { // 1
    curr_poz=pont_move(curr_poz, curr_dir);
    curr_csucs=csucs_move(curr_csucs, curr_dir);
    known_csucs(curr_csucs, what, curr_dir, curr_poz, csfej, 1);
  }
  }

int main(int argc, char **argv) {
  //  int liw;
  init();

  curr_poz.x=0;
  curr_poz.y=0;
  curr_csucs=start_csucs=new_csucs();
  csfej=cslist_add(NULL, curr_poz, curr_csucs);
  curr_dir=0;
  known_csucs(curr_csucs, "010\01", curr_dir, curr_poz, csfej, 0);
  while (1) { // ciklus invarians: az alattunk levo csucs mar ismert, olyan iranyba allunk, amerre akarunk menni
    elfest(curr_csucs, curr_dir);
    if (csucs_move(curr_csucs, curr_dir)->state==0) { // ismeretlen csucs jon
      unknown_csucs(junction_search(1));
      if (curr_csucs->siblings[(curr_dir+3)%4]!=NULL && curr_csucs->elfestes[(curr_dir+3)%4]==0) { // lehet balra menni
	curr_dir=(curr_dir+3)%4;
	left_turn();
      } else if (curr_csucs->siblings[curr_dir]!=NULL && curr_csucs->elfestes[curr_dir]==0) { // lehet elore menni
	no_turn();
      } else if (curr_csucs->siblings[(curr_dir+1)%4]!=NULL && curr_csucs->elfestes[(curr_dir+1)%4]==0) { // lehet jobbra menni
	curr_dir=(curr_dir+1)%4;
	right_turn();
      } else {// zsakutca, meg kell fordulni
	curr_dir=(curr_dir+2)%4;
	around_turn();
      }
    } else { // ismeros csucs jon
      if (curr_csucs->elfestes[curr_dir]==1) { // oda 1-as festesu visz
	// ilyen elvileg sosincs, mert kezeljuk menetkozben
	cputs("WTF1");
	csucs_dump(start_csucs, csucs_dump_pointer, 0, csucs_festes++);
	while(!shutdown_requested()) {
	  lnp_integrity_write(csucs_dump_pointer,strlen(csucs_dump_pointer));
	  delay(1000);
	  dsound_system(0);
	}

	deinit();
      } else if (curr_csucs->elfestes[curr_dir]==2) { // oda 2-es festesu visz
	// menjunk oda
	junction_search(0);
	curr_poz=pont_move(curr_poz, curr_dir);
	curr_csucs=csucs_move(curr_csucs, curr_dir);
	// it can be non visible for the robot, double length
	if (curr_csucs->siblings[curr_dir]!=NULL && curr_csucs->siblings[(curr_dir+3)%4]==NULL && curr_csucs->siblings[(curr_dir+1)%4]==NULL) {
	  elfest(curr_csucs, curr_dir);
	  curr_poz=pont_move(curr_poz, curr_dir);
	  curr_csucs=csucs_move(curr_csucs, curr_dir);
	} 
	// probalunk keresni egy ismeretlen szomszedot (0-as festesut)
	if (curr_csucs->siblings[(curr_dir+3)%4]!=NULL && curr_csucs->elfestes[(curr_dir+3)%4]==0) { // lehet balra menni
	  curr_dir=(curr_dir+3)%4;
	  left_turn();
	} else if (curr_csucs->siblings[curr_dir]!=NULL && curr_csucs->elfestes[curr_dir]==0) { // lehet elore menni
	  no_turn();
	} else if (curr_csucs->siblings[(curr_dir+1)%4]!=NULL && curr_csucs->elfestes[(curr_dir+1)%4]==0) { // lehet jobbra menni
	  curr_dir=(curr_dir+1)%4;
	  right_turn();
	}
	// keresunk egy egyes fetesu utat es azon visszamegyunk
	else if (curr_csucs->siblings[(curr_dir+3)%4]!=NULL && curr_csucs->elfestes[(curr_dir+3)%4]==1) { // lehet balra menni
	  curr_dir=(curr_dir+3)%4;
	  left_turn();
	} else if (curr_csucs->siblings[curr_dir]!=NULL && curr_csucs->elfestes[curr_dir]==1) { // lehet elore menni
	  no_turn();
	} else if (curr_csucs->siblings[(curr_dir+1)%4]!=NULL && curr_csucs->elfestes[(curr_dir+1)%4]==1) { // lehet jobbra menni
	  curr_dir=(curr_dir+1)%4;
	  right_turn();
	} else {// olyan csucsban vagyunk, akinek csak 2-es festesu elei vannak, azaz visszaertunk a startba!
	  break;
	}
      }
    }
    /*    csucs_dump(start_csucs, csucs_dump_pointer, 0, csucs_festes++);
    dsound_system(0);
    for (liw=0;liw<3;liw++) {
      lnp_integrity_write(csucs_dump_pointer,strlen(csucs_dump_pointer));
      delay(100);
      }*/
  }
  csucs_dump(start_csucs, csucs_dump_pointer, 0, csucs_festes++);
  while(!shutdown_requested()) {
    lnp_integrity_write(csucs_dump_pointer,strlen(csucs_dump_pointer));
    delay(1000);
    dsound_system(0);
  }

  deinit();
  
  return 0;
}