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

void unknown_csucs(char* what) {
  if (what[4]==2) {
    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;
//  char* elagazas;

  ds_active(&SENSOR_1);
  ds_active(&SENSOR_2);
  ds_active(&SENSOR_3);
  delay(300);

  /*megfordul();
  return 0;
      // elagazaskereso
  elagazas=elagazaskeres();
  elagazas[3]=elagazas[4]+48;
  elagazas[4]=0;
  cputs(elagazas);
  return 0;*/
  
  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(elagazaskeres());
      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;
	balra_fordul();
      } else if (curr_csucs->siblings[curr_dir]!=NULL && curr_csucs->elfestes[curr_dir]==0) { // lehet elore menni
	keresztezodest_elhagy();
      } 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;
	jobbra_fordul();
      } else {// zsakutca, meg kell fordulni
	curr_dir=(curr_dir+2)%4;
	megfordul();
      }
    } else { // ismeros csucs jon
      if (curr_csucs->elfestes[curr_dir]==1) { // oda 1-as festesu visz
	// ilyen elvileg sosincs, mert kezeljuk menetkozben
	cputs("WTF1");
	exit(1);
      } else if (curr_csucs->elfestes[curr_dir]==2) { // oda 2-es festesu visz
	// menjunk oda
	elagazaskeres();
	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;
	  balra_fordul();
	} else if (curr_csucs->siblings[curr_dir]!=NULL && curr_csucs->elfestes[curr_dir]==0) { // lehet elore menni
	  keresztezodest_elhagy();
	} 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;
	  jobbra_fordul();
	}
	// 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;
	  balra_fordul();
	} else if (curr_csucs->siblings[curr_dir]!=NULL && curr_csucs->elfestes[curr_dir]==1) { // lehet elore menni
	  keresztezodest_elhagy();
	} 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;
	  jobbra_fordul();
	} 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);
  }
  ds_passive(&SENSOR_1);
  ds_passive(&SENSOR_2);
  ds_passive(&SENSOR_3);

  return 0;
}