vizsga

//#include <stdio.h>


const int BLACK = 1;
const int  BLUE = 2;
const int  GREEN = 3;
const int  YELLOW = 4;
const int  RED = 5;
const int  WHITE = 6;

const int  COLOR_FOLLOW = WHITE;
const int  COLOR_WALL = RED;
const int  COLOR_INTERSECTION = YELLOW;
const int  COLOR_BLOCKMARK = GREEN;
const int  COLOR_MAZE_END = YELLOW;

const int BLOCK_WIDTH_CM = 20;
const int BLOCK_LENGTH_CM = 20;

int getColor() {
  int color;
  //printf("What color are you seeing?\n");
  //scanf("%d", &color);
  color = Sensor(IN_3);
  NumOut(LCD_LINE2, LCD_LINE2, color, false);
  return color;
}

void turnLeft() {
  //printf("Turning left 90°\n");
  OnFwdSync(OUT_AC, 32, 90);
  Wait(1000);
}

void turnRight() {
  //printf("Turning left 90°\n");
  OnFwdSync(OUT_AC, 32, -90);
  Wait(1000);
}

void turnBack() {
  //printf(“Turning back 180°\n”);
  OnFwdSync(OUT_AC, 32, 180);
  Wait(1000);
}

void goForward() {
  //printf("Going forward\n");
  OnFwdSync(OUT_AC, 70, 0);

}


void goForwardCm(int cm) {
  //printf("Going forward %d cms\n", cm);
  int fordulat = 0;
  int b = 0;
  ResetTachoCount(OUT_A);
  OnFwdSync(OUT_AC, 70, 0);
  while(b == 0) {
    fordulat += MotorTachoCount(OUT_A);
    if (fordulat >= cm * 360) {
      b = 1;
    }
  }
  OnFwdSync(OUT_AC, 0, 0);
}

void goBackCm(int cm) {
  //printf("Going forward %d cms\n", cm);
  int fordulat = 0;
  int b = 0;
  ResetTachoCount(OUT_A);
  OnFwdSync(OUT_AC, -70, 0);
  while(b == 0) {
    fordulat += MotorTachoCount(OUT_A);
    if (fordulat >= cm * 360) {
      b = 1;
    }
  }
  OnFwdSync(OUT_AC, 0, 0);
}

void backOff() {
  goBackCm(5);
}


void followLine() {
  //printf("Following line\n");
  goForward();
}


void crossIntersection() {
  //printf("Crossing an intersection\n");
  while (getColor() != COLOR_FOLLOW) {
    goForward();
    Wait(10);
  }
}


void avoidBlock() {
  //printf("Avoiding a block\n");
  int distance = SensorUS(IN_2);
  while (distance > 10) {
    goForward();
    distance = SensorUS(IN_2);
  }
  OnFwdSync(OUT_AC, 0, 0);
  turnRight();
  goForwardCm(BLOCK_WIDTH_CM / 2);
  turnLeft();
  goForwardCm(BLOCK_LENGTH_CM);
  turnLeft();
  goForwardCm(BLOCK_WIDTH_CM / 2);
  turnRight();
  goForward();
}

void turnSequence() {
  turnRight();
  backOff();
  if(getColor() == COLOR_WALL) {
    turnBack();
    backOff();
    if(getColor() == COLOR_WALL) {
      turnLeft();
      backOff();
    }
  }
}

void solveMaze() {
  //printf("Solving a maze\n");
  //TODO detection of entering the maze
  int b = 1;
  while(b) {
    goForward();
    Wait(100);
    if(getColor() == COLOR_WALL) {
      turnSequence();
    }
  //TODO detection of the end of the maze
  }
}

void searchLine() {
  //printf("Searching for a line\n");


  unsigned int turn = 0; // fordulasi erosseg
  int turnway = 1; // fordulasi irany
  int tpower = 0; // fordulasi erosseg az irannyal "sulyozva"
  while (getColor() != COLOR_FOLLOW) {
      if (turn % 30 == 0) {
        turnway *= -2;
      }
      tpower = ((turn % 50) + 50) * turnway;
      OnFwdSync(OUT_AC, 30, tpower);
      Wait(100);
      turn++;
  }
}

task main() {
  int color;
  string test;

  SetSensorColorFull(IN_3);
  SetSensorLowspeed(IN_2);

  while (1) {
    color = getColor();

    switch (color) {
      case COLOR_FOLLOW: strcpy(test, "follow"); TextOut(0, LCD_LINE1, test); followLine(); break;
      case COLOR_INTERSECTION: strcpy(test, "cross"); TextOut(0, LCD_LINE1, test); crossIntersection(); break;
      case COLOR_BLOCKMARK: strcpy(test, "block"); TextOut(0, LCD_LINE1, test); avoidBlock(); break;
      case COLOR_WALL: strcpy(test, "maze"); TextOut(0, LCD_LINE1, test); solveMaze(); break;
      default: strcpy(test, "searching"); TextOut(0, LCD_LINE1, test); searchLine(); break;
    }
    Wait(10);
  }
}