// This version uses bit-banged SPI.
// If you see tearing (jagged edges on the circles) try the version
// which uses AVR's hardware SPI peripheral:
// https://wokwi.com/arduino/projects/318868939929027156
#include <Servo.h>

#define CLK 13
#define DIN 11
#define CS  10
#define X_SEGMENTS   4
#define Y_SEGMENTS   4
#define NUM_SEGMENTS (X_SEGMENTS * Y_SEGMENTS)

// a framebuffer to hold the state of the entire matrix of LEDs
// laid out in raster order, with (0, 0) at the top-left
byte fb[8 * NUM_SEGMENTS];

Servo servo;


void shiftAll(byte send_to_address, byte send_this_data)
{
  digitalWrite(CS, LOW);
  for (int i = 0; i < NUM_SEGMENTS; i++) {
    shiftOut(DIN, CLK, MSBFIRST, send_to_address);
    shiftOut(DIN, CLK, MSBFIRST, send_this_data);
  }
  digitalWrite(CS, HIGH);
}


void setup() {
  Serial.begin(115200);
  pinMode(CLK, OUTPUT);
  pinMode(DIN, OUTPUT);
  pinMode(CS, OUTPUT);
   servo.attach(9); // Замените SERVO_PIN на пин, к которому подключен ваш сервопривод

  // Setup each MAX7219
  shiftAll(0x0f, 0x00); //display test register - test mode off
  shiftAll(0x0b, 0x07); //scan limit register - display digits 0 thru 7
  shiftAll(0x0c, 0x01); //shutdown register - normal operation
  shiftAll(0x0a, 0x0f); //intensity register - max brightness
  shiftAll(0x09, 0x00); //decode mode register - No decode
  clear_memory();
}

const int angles[] = {0, 45, 90, 135};

#define UP    0
#define RIGHT 1
#define DOWN  2
#define LEFT  3



// Пример матрицы 30x30 (0 - пустое место, 1 - стена)
int matrix[32][32] = {
    {1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1},
    {1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1},
    {1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1},
    {1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1},
    {1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1},
    {1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1},
    {1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1},
    {1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1},
    {1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1},
    {1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1},
    {1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1},
    {1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1},
    {1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1},
    {1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1},
    {1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1},
    {1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1},
    {1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1},
    {1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1},
    {1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1},
    {1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1},
    {1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1},
    {1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1},
    {1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1},
    {1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1},
    {1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1},
    {1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1},
    {1, 0, 0, 0, 0, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1},
    {1, 0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1},
    {1, 0, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1},
    {1, 0, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1},
    {1, 0, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1},
    {1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1}

};

// Матрица для запоминания стенок (0 - не посещено, 1 - стена)
int memory[32][32] = {0};

void clear_memory() {
  memset(memory, 0, sizeof(memory));
}


void loop() {
  static int16_t robot_x = 16; // Начальное положение робота по x
  static int16_t robot_y = 16; // Начальное положение робота по y
  static int16_t direction = RIGHT; // Начальное направление движения робота
  static bool wall_touch = false; // Переменная для отслеживания прижатия к стене
   servo.write(angles[direction]);
  // Очистка буфера кадра
  clear();

  // Отрисовка робота в текущем положении
  safe_pixel(robot_x, robot_y, 1);
 

  // Испускание лучей в четыре направления
  int16_t beam_x = robot_x;
  int16_t beam_y = robot_y;
  
  // Луч вверх
  for (int16_t y = robot_y - 1; y >= 0; y--) {
    if (matrix[y][robot_x] == 1) {
      memory[y][robot_x] = 1;
      beam_y = y + 1; // Пиксель после стены
      break;
    }
  }
  safe_pixel(robot_x, beam_y, 1);
  //  safe_pixel(robot_x, beam_y+32, 1);

  // Луч вправо
  for (int16_t x = robot_x + 1; x < 32; x++) {
    if (matrix[robot_y][x] == 1) {
      memory[robot_y][x] = 1;
      beam_x = x - 1; // Пиксель перед стеной
      break;
    }
  }
  safe_pixel(beam_x, robot_y, 1);

  // Луч вниз
  for (int16_t y = robot_y + 1; y < 32; y++) {
    if (matrix[y][robot_x] == 1) {
      memory[y][robot_x] = 1;
      beam_y = y - 1; // Пиксель перед стеной
      break;
    }
  }
  safe_pixel(robot_x, beam_y, 1);

  // Луч влево
  for (int16_t x = robot_x - 1; x >= 0; x--) {
    if (matrix[robot_y][x] == 1) {
      memory[robot_y][x] = 1;
      beam_x = x + 1; // Пиксель после стены
      break;
    }
  }
  safe_pixel(beam_x, robot_y, 1);

  // Отображаем запомненные стены
  for (int16_t y = 0; y < 32; y++) {
    for (int16_t x = 0; x < 32; x++) {
      if (memory[y][x] == 1) {
        safe_pixel(x, y, 1);
      }
    }
  }

  // Движение робота в текущем направлении, если следующая клетка не стена
  int16_t next_x = robot_x;
  int16_t next_y = robot_y;

  switch (direction) {
    case UP:
      next_y--;
      break;
    case RIGHT:
      next_x++;
      break;
    case DOWN:
      next_y++;
      break;
    case LEFT:
      next_x--;
      break;
  }
// Проверка, является ли следующая клетка допустимой
if (next_x >= 0 && next_x < 32 && next_y >= 0 && next_y < 32 && matrix[next_y][next_x] == 0) {

    if(wall_touch){
    matrix[robot_y][robot_x] = 1;
    }
    
    // Если следующая клетка не стена, двигаем робота в этом направлении
    robot_x = next_x;
    robot_y = next_y;

    // Если робот прошел мимо стены, проверяем, есть ли стена слева
    int16_t left_x, left_y;
    switch (direction) {
        case UP:
            left_x = robot_x - 1;
            left_y = robot_y;
            break;
        case RIGHT:
            left_x = robot_x;
            left_y = robot_y - 1;
            break;
        case DOWN:
            left_x = robot_x + 1;
            left_y = robot_y;
            break;
        case LEFT:
            left_x = robot_x;
            left_y = robot_y + 1;
            break;
    }
    if (left_x >= 0 && left_x < 32 && left_y >= 0 && left_y < 32 && matrix[left_y][left_x] == 1) {
        wall_touch = true; // Стена слева
    } else {
        // wall_touch = false; // Нет стены слева
    }
} else {
    // Если следующая клетка стена или выходит за границы матрицы, повернуть направо
    direction = (direction + 1) % 4;
}

// Если робот прижат к стене, попробуем повернуть налево, если нет стены слева
if (wall_touch) {
    int16_t left_x, left_y;
    switch (direction) {
        case UP:
            left_x = robot_x - 1;
            left_y = robot_y;
            break;
        case RIGHT:
            left_x = robot_x;
            left_y = robot_y - 1;
            break;
        case DOWN:
            left_x = robot_x + 1;
            left_y = robot_y;
            break;
        case LEFT:
            left_x = robot_x;
            left_y = robot_y + 1;
            break;
    }
    if (left_x >= 0 && left_x < 32 && left_y >= 0 && left_y < 32 && matrix[left_y][left_x] == 0) {
        direction = (direction + 3) % 4; // Поворот налево
        // wall_touch = false; // Сбрасываем флаг прижатия к стене после поворота налево
    }
}

  // Отображение буфера кадра
  show();

  // Задержка для плавной анимации
  delay(100);
}




void set_pixel(uint8_t x, uint8_t y, uint8_t mode) {
  byte *addr = &fb[x / 8 + y * X_SEGMENTS];
  byte mask = 128 >> (x % 8);
  switch (mode) {
    case 0: // clear pixel
      *addr &= ~mask;
      break;
    case 1: // plot pixel
      *addr |= mask;
      break;
    case 2: // XOR pixel
      *addr ^= mask;
      break;
  }
}


void safe_pixel(uint8_t x, uint8_t y, uint8_t mode) {
  if ((x >= X_SEGMENTS * 8) || (y >= Y_SEGMENTS * 8))
    return;
  set_pixel(x, y, mode);
}


// turn off every LED in the framebuffer
void clear() {
  byte *addr = fb;
  for (byte i = 0; i < 8 * NUM_SEGMENTS; i++)
    *addr++ = 0;
}


// send the raster order framebuffer in the correct order
// for the boustrophedon layout of daisy-chained MAX7219s
void show() {
  for (byte row = 0; row < 8; row++) {
    digitalWrite(CS, LOW);
    byte segment = NUM_SEGMENTS;
    while (segment--) {
      byte x = segment % X_SEGMENTS;
      byte y = segment / X_SEGMENTS * 8;
      byte addr = (row + y) * X_SEGMENTS;

      if (segment & X_SEGMENTS) { // odd rows of segments
        shiftOut(DIN, CLK, MSBFIRST, 8 - row);
        shiftOut(DIN, CLK, LSBFIRST, fb[addr + x]);
      } else { // even rows of segments
        shiftOut(DIN, CLK, MSBFIRST, 1 + row);
        shiftOut(DIN, CLK, MSBFIRST, fb[addr - x + X_SEGMENTS - 1]);
      }
    }
    digitalWrite(CS, HIGH);
  }
}