#include <conio.h> 
#include <string.h> 
#include <unistd.h> 
#include <dsensor.h> 
#include <stdlib.h> 
#include <unistd.h> 
#include <dmotor.h> 
#include <dsound.h> 
#include <lnp.h> 

// bal oldali:
// 2d felett feher
// 28 alatt fekete
// kozepso:
// 2d felett feher
// 28 alatt fekete
// jobb oldali:
// 30 felett feher
// 2b alatt fekete
#define TH_LEFT_WHITE 0x2D 
#define TH_LEFT_BLACK 0x28 
#define TH_MID_WHITE 0x2D 
#define TH_MID_BLACK 0x28 
#define TH_RIGHT_WHITE 0x30 
#define TH_RIGHT_BLACK 0x2B 
#define LIGHT_LEFT LIGHT(SENSOR_1) 
#define LIGHT_MID LIGHT(SENSOR_2) 
#define LIGHT_RIGHT LIGHT(SENSOR_3) 

#define ELOREMEGY_KORRIG_RANT 10 
#define ELOREMEGY_RANT 12 
#define FORDUL_RANT 16 
#define FORDUL_ELORE 9 

void set_speed(int what) {
  motor_a_speed(what);
  motor_c_speed(what);
}

void megall() {
  set_speed(0);
  motor_a_dir(off);
  motor_c_dir(off);
  delay(100);
}

void jobbra_rant(int k) {
  motor_a_dir(fwd);
  motor_c_dir(rev);
  set_speed(250);
  delay(k);
  megall();
}

void elore_rant(int k) {
  motor_a_dir(fwd);
  motor_c_dir(fwd);
  set_speed(250);
  delay(k);
  megall();
}

void balra_rant(int k) {
  motor_a_dir(rev);
  motor_c_dir(fwd);
  set_speed(250);
  delay(k);
  megall();
}

int eloremegy() {
  int ret=0;
  while (LIGHT_LEFT > TH_LEFT_BLACK && LIGHT_RIGHT > TH_RIGHT_BLACK && LIGHT_MID <= TH_MID_BLACK) {
    if (LIGHT_LEFT < TH_LEFT_WHITE) {
      balra_rant(ELOREMEGY_KORRIG_RANT);
      if (LIGHT_LEFT < TH_LEFT_WHITE) {
	// ha nem sikerult helyrehoznunk, akkor inkabb csinaljuk vissza
	jobbra_rant(ELOREMEGY_KORRIG_RANT);
      }
    }
    else if (LIGHT_RIGHT < TH_RIGHT_WHITE) {
      jobbra_rant(ELOREMEGY_KORRIG_RANT);
      if (LIGHT_RIGHT < TH_RIGHT_WHITE) {
	// ha nem sikerult helyrehoznunk, akkor inkabb csinaljuk vissza
	balra_rant(ELOREMEGY_KORRIG_RANT);
      }
    }
    while (LIGHT_LEFT >= TH_LEFT_WHITE && LIGHT_RIGHT >= TH_RIGHT_WHITE && LIGHT_MID <= TH_MID_BLACK) {
      elore_rant(ELOREMEGY_RANT);
      ret++;
    }
    if (LIGHT_RIGHT <= TH_RIGHT_BLACK) { // jobb oldalon elagazas lehet, de lehet, hogy csak durva lecsuszas
      balra_rant(FORDUL_RANT);
      if (LIGHT_MID > TH_MID_BLACK) { // tenyleg csak durva lecsuszas
	jobbra_rant(FORDUL_RANT);
	while (!(LIGHT_RIGHT >= TH_RIGHT_WHITE)) jobbra_rant(FORDUL_RANT/2);
      } else { // ez egy elagazas lesz
	jobbra_rant(FORDUL_RANT);
      }
    }
    if (LIGHT_LEFT <= TH_LEFT_BLACK) { // bal oldalon elagazas lehet, de lehet, hogy csak durva lecsuszas
      jobbra_rant(FORDUL_RANT);
      if (LIGHT_MID > TH_MID_BLACK) { // tenyleg csak durva lecsuszas
	balra_rant(FORDUL_RANT);
	while (!(LIGHT_LEFT >= TH_LEFT_WHITE)) balra_rant(FORDUL_RANT/2);
      } else { // ez egy elagazas lesz
	balra_rant(FORDUL_RANT);
      }
    }
  }
  return ret;
}

void jobbra_fordul() {
  int i;

  cputs("RIGH");
  for (i=0;i<FORDUL_ELORE;i++) elore_rant(ELOREMEGY_RANT);
  jobbra_rant(FORDUL_RANT);
  jobbra_rant(FORDUL_RANT);
  while (LIGHT_MID <= TH_MID_BLACK) jobbra_rant(FORDUL_RANT);
  while (!(LIGHT_MID <= TH_MID_BLACK && LIGHT_RIGHT >= TH_RIGHT_WHITE)) jobbra_rant(FORDUL_RANT);
  jobbra_rant(FORDUL_RANT/2);
  while (!(LIGHT_LEFT >= TH_LEFT_WHITE)) balra_rant(FORDUL_RANT/2);
}

void balra_fordul() {
  int i;

  cputs("LEFT");
  for (i=0;i<FORDUL_ELORE;i++) elore_rant(ELOREMEGY_RANT);
  balra_rant(FORDUL_RANT);
  balra_rant(FORDUL_RANT);
  while (LIGHT_MID <= TH_MID_BLACK) balra_rant(FORDUL_RANT);
  while (!(LIGHT_MID <= TH_MID_BLACK && LIGHT_LEFT >= TH_LEFT_WHITE)) balra_rant(FORDUL_RANT);
  balra_rant(FORDUL_RANT/2);
  while (!(LIGHT_RIGHT >= TH_RIGHT_WHITE)) jobbra_rant(FORDUL_RANT/2);
}

void megfordul() {
  int i;

  cputs("TURN");
  for (i=0;i<FORDUL_ELORE;i++) elore_rant(ELOREMEGY_RANT);
  jobbra_rant(FORDUL_RANT);
  jobbra_rant(FORDUL_RANT);
  while (LIGHT_MID <= TH_MID_BLACK) jobbra_rant(FORDUL_RANT);
  while (!(LIGHT_MID <= TH_MID_BLACK && LIGHT_RIGHT >= TH_RIGHT_WHITE)) jobbra_rant(FORDUL_RANT);
  while (LIGHT_MID <= TH_MID_BLACK) jobbra_rant(FORDUL_RANT);
  while (!(LIGHT_MID <= TH_MID_BLACK && LIGHT_RIGHT >= TH_RIGHT_WHITE)) jobbra_rant(FORDUL_RANT);
  jobbra_rant(FORDUL_RANT/2);
  while (!(LIGHT_LEFT >= TH_LEFT_WHITE)) balra_rant(FORDUL_RANT/2);
}

static char* elagazaskeres() {
  static char ret[5];
  int tav;

  cputs("SEAR");
  tav=eloremegy();
  cputs("FIND");
  cputw(tav);
  if (tav>0x1d)
    ret[4]=2;
  else
    ret[4]=1;
  elore_rant((ELOREMEGY_RANT*3)/2);
  balra_rant(FORDUL_RANT);
  if (LIGHT_LEFT <= TH_LEFT_BLACK) ret[0]='1';
  else ret[0]='0';
  jobbra_rant(FORDUL_RANT);
  jobbra_rant(FORDUL_RANT);
  if (LIGHT_RIGHT <= TH_RIGHT_BLACK) ret[2]='1';
  else ret[2]='0';
  balra_rant(FORDUL_RANT);
  if (LIGHT_MID <= TH_MID_BLACK) ret[1]='1';
  else ret[1]='0';

  return ret;
}

void keresztezodest_elhagy() {
  int i;

  cputs("AHEA");
  for (i=0;i<FORDUL_ELORE;i++) elore_rant(ELOREMEGY_RANT);
  while (LIGHT_MID < TH_MID_WHITE)
    jobbra_rant(FORDUL_RANT);
  while (!(LIGHT_MID <= TH_MID_BLACK && LIGHT_LEFT >= TH_LEFT_WHITE))
    balra_rant(FORDUL_RANT);
  balra_rant(FORDUL_RANT/2);
  while (!(LIGHT_RIGHT >= TH_RIGHT_WHITE)) jobbra_rant(FORDUL_RANT/2);  
}