/*
bool nodeMap[7][8] = {
  {0,0,0,0,0,0,0,0},
  {0,0,0,0,1,0,0,0},
  {0,0,1,0,1,0,0,0},
  {0,1,1,1,1,0,0,0},
  {0,0,1,0,1,1,1,0},
  {0,0,0,0,1,0,0,0},
  {0,0,0,0,0,0,0,0}
};

bool start = true;
struct nNode{
  int x;
  int y;
};

nNode currentNode;
nNode nextNode;
//currentNode.x = 4;
//currentNode.y = 5;

void printNodeMap() {
    Serial.println("Карта узлов (1 - проходимо, 0 - стена):");
    for (int i = 0; i < 7; i++) {  // Перебираем строки (Y)
        for (int j = 0; j < 8; j++) {  // Перебираем столбцы (X)
            Serial.print(nodeMap[i][j]);  
            Serial.print(" ");
        }
        Serial.println(); // Перенос строки после вывода строки карты
    }
    Serial.println(); // Дополнительный отступ для читабельности
}


void movementKit(){
Serial.print("x: ");
Serial.print(currentNode.x);
Serial.print(" -> ");
Serial.println(nextNode.x);
Serial.print("y: ");
Serial.print(currentNode.y);
Serial.print(" -> ");
Serial.println(nextNode.y);
currentNode.x = nextNode.x;
currentNode.y = nextNode.y;
}

void nodeNavigator(int goalY, int goalX){
  while(!(goalX == currentNode.x && goalY == currentNode.y)){
  float mostValue = 99;
  float forwardValue = 99;
  float backwardValue = 99;
  float rightValue = 99;
  float leftValue = 99; 

  if (nodeMap[currentNode.y][currentNode.x+1]){rightValue = abs(currentNode.y-goalY)+abs(currentNode.x+1-goalX);}
  if (nodeMap[currentNode.y][currentNode.x-1]){leftValue = abs(currentNode.y-goalY)+abs(currentNode.x-1-goalX);}
  if (nodeMap[currentNode.y+1][currentNode.x]){backwardValue = abs(currentNode.y+1-goalY)+abs(currentNode.x-goalX);}
  if (nodeMap[currentNode.y+1][currentNode.x]){forwardValue = abs(currentNode.y-1-goalY)+abs(currentNode.x-goalX);}
  Serial.println("mostValue :");
  Serial.println(mostValue);
    Serial.println("forwardValue :");
  Serial.println(forwardValue);
    Serial.println("backValue :");
  Serial.println(backwardValue);
    Serial.println("leftValue :");
  Serial.println(leftValue);
    Serial.println("rightValue :");
  Serial.println(rightValue);
  
  if(forwardValue<backwardValue){mostValue = forwardValue;}
  else{mostValue = backwardValue;}
  if(leftValue<mostValue){mostValue=leftValue;}
  if(rightValue<mostValue){mostValue=rightValue;}

  if (mostValue == backwardValue){
  nextNode.x=currentNode.x;
  nextNode.y=currentNode.y + 1;
  }

  else if (mostValue == forwardValue){
  nextNode.x = currentNode.x;
  nextNode.y = currentNode.y - 1; 
  }

  else if (mostValue == leftValue){
  nextNode.x = currentNode.x - 1;
  nextNode.y = currentNode.y; 
  }

  else  if (mostValue == rightValue){
  nextNode.x = currentNode.x + 1;
  nextNode.y = currentNode.y; 
      }

        Serial.println("mostValue :");
  Serial.println(mostValue);

  movementKit();

    }
  }



void setup() {
currentNode.x = 4;
currentNode.y = 5;
nextNode.x = 4;
nextNode.y = 4;
Serial.begin(9600);
printNodeMap();
}
//void startCurrentInitialization(){
//currentNode.x = 4;
///currentNode.y = 5;
//start = false;
//}
void loop() {
 // if (start){startCurrentInitialization();}
for(int i = 0; i < 7; i++){
  for(int j = 0; j < 8; j++){
    Serial.print('(');
    Serial.print(i);
    Serial.print(';');
    Serial.print(j);
    Serial.print(") ");
    if(j==7){Serial.println(' ');}
  }
}
for(int i = 0; i < 7; i++){
  for(int j = 0; j < 8; j++){
    Serial.print(" ");
    Serial.print(nodeMap[j][i]);
    Serial.print(" ");
    if(j==7){Serial.println(' ');}}

nodeNavigator(4, 6);

}
}
*/
bool nodeMap[7][8] = {
  {0,0,0,0,0,0,0,0},
  {0,0,0,0,1,0,0,0},
  {0,0,1,0,1,0,0,0},
  {0,1,1,1,1,0,0,0},
  {0,0,1,0,1,1,1,0},
  {0,0,0,0,1,0,0,0},
  {0,0,0,0,0,0,0,0}
};

struct nNode {
    int x;
    int y;
};

nNode currentNode;
nNode nextNode;

void movementKit() {
    Serial.print("x: ");
    Serial.print(currentNode.x);
    Serial.print(" -> ");
    Serial.println(nextNode.x);
    Serial.print("y: ");
    Serial.print(currentNode.y);
    Serial.print(" -> ");
    Serial.println(nextNode.y);
    Serial.println(' ');
    currentNode = nextNode; // Обновляем текущий узел
}

void nodeNavigator(int goalY, int goalX) {
    while (!(goalX == currentNode.x && goalY == currentNode.y)) {
        float minDistance = 99; // Изначально задаем большое значение
        int bestX = currentNode.x;
        int bestY = currentNode.y;

        // Проверяем движение вправо
        if (currentNode.x + 1 < 8 && nodeMap[currentNode.y][currentNode.x + 1]) {
            float distance = abs(currentNode.y - goalY) + abs(currentNode.x + 1 - goalX);
            if (distance < minDistance) {
                minDistance = distance;
                bestX = currentNode.x + 1;
                bestY = currentNode.y;
            }
        }

        // Проверяем движение влево
        if (currentNode.x - 1 >= 0 && nodeMap[currentNode.y][currentNode.x - 1]) {
            float distance = abs(currentNode.y - goalY) + abs(currentNode.x - 1 - goalX);
            if (distance < minDistance) {
                minDistance = distance;
                bestX = currentNode.x - 1;
                bestY = currentNode.y;
            }
        }

        // Проверяем движение вниз
        if (currentNode.y + 1 < 7 && nodeMap[currentNode.y + 1][currentNode.x]) {
            float distance = abs(currentNode.y + 1 - goalY) + abs(currentNode.x - goalX);
            if (distance < minDistance) {
                minDistance = distance;
                bestX = currentNode.x;
                bestY = currentNode.y + 1;
            }
        }

        // Проверяем движение вверх
        if (currentNode.y - 1 >= 0 && nodeMap[currentNode.y - 1][currentNode.x]) {
            float distance = abs(currentNode.y - 1 - goalY) + abs(currentNode.x - goalX);
            if (distance < minDistance) {
                minDistance = distance;
                bestX = currentNode.x;
                bestY = currentNode.y - 1;
            }
        }

        // Если координаты не изменились, значит зашли в тупик
        if (bestX == currentNode.x && bestY == currentNode.y) {
            Serial.println("Робот застрял! Нет доступных путей.");
            break;
        }

        // Обновляем координаты
        nextNode.x = bestX;
        nextNode.y = bestY;

        movementKit();
    }
}

void setup() {
    Serial.println("Next path:");
    Serial.begin(9600);
    currentNode.x = 4;
    currentNode.y = 5;
    nextNode.x = 4;
    nextNode.y = 4;
    Serial.println("Next path:");
    nodeNavigator(3, 1);
    Serial.println("Next path:");
    nodeNavigator(1, 4);
    //nodeNavigator(4, 6);
    //nodeNavigator(5, 4); // Строим путь к цели (4,6)
}

void loop() {
}