AoC 2021 day 15 part 1

#include <Arduino.h>
#include "data.h"
const int gridSize = 100;
const int CLOSED_SIZE = 2000;
const int OPEN_SIZE = 150;

/*
  data looks like this:
  const uint8_t data[] PROGMEM ={7,9,7,2,9,8,1,6,8,8,7,9,1,3,1,7,7,...};
*/

struct Node {
  byte x=0;
  byte y=0;

  int f = 0; // g+h;
  int g = 9999; // cost so far
  byte h = 0; // distance to goal

  bool inOpen = false;
};

struct Graph{
  Node nodes[OPEN_SIZE];
  int index = 0;
  int closed[CLOSED_SIZE] = {0};
  int indexClosed = 0;

};

Graph graph;
int neighbors[4][2] = {{-1,0},{1,0},{0,-1},{0,1}};

int endX = gridSize-1;
int endY = gridSize-1;
void unlockNode(int x, int y){

  graph.nodes[graph.index].x = x;
  graph.nodes[graph.index].y = y;


  graph.nodes[graph.index].h =  (endX - x) + (endY - y);
  graph.index++;
}

bool isClosed(int x, int y){

  for (int i = 0; i < CLOSED_SIZE; i++){
    if( graph.closed[i] == y*gridSize+x){
      return true;
    }
  }
  return false;
}

void removeNode(int x, int y){
  bool found = false;
  for (int i = 0; i < graph.index; i++)
  {
    if(graph.nodes[i].x == x && graph.nodes[i].y == y)
      found = true;

    if(found){
        memcpy( &graph.nodes[i], &graph.nodes[i+1], sizeof graph.nodes[i] );
    }
  }
  graph.index--;
  graph.closed[graph.indexClosed] = y*gridSize+x;
  graph.indexClosed++;
  if(graph.indexClosed > CLOSED_SIZE)
    graph.indexClosed = 0;

}

Node* getNode(int x, int y);
Node* getNode(int x, int y){
  for (int i = 0; i < graph.index; i++)
  {
    if(graph.nodes[i].x == x && graph.nodes[i].y == y)
      return &graph.nodes[i];
  }
  unlockNode(x,y);
  return &graph.nodes[graph.index-1];
}

Node* getLowestCostNode();
Node* getLowestCostNode(){
  int lowest = INFINITY;
  Node* n;
  for (int i = 0; i < graph.index; i++)
  {
    if(graph.nodes[i].f<lowest){
      lowest = graph.nodes[i].f;
      n = &graph.nodes[i];
    }

  }
  return n;
}

void setup() {

  Serial.begin(9600);
  while(!Serial);

  long t=millis(); // get current time

  Node* n = getNode(0,0);
  n->inOpen = true;
  n->g = 0;
  bool destination = false;
  while (destination == false){

    Node* n = getLowestCostNode();
    if (n->x == endX && n->y == endY){
      destination = true;
      Serial.println("DONE!!!!");
      Serial.println(n->g);
    }
    else{

      n->inOpen = false;

      for (auto edge : neighbors){

        // int f = g+h;
        // int g = cost so far
        // int h = distance to goal
        int x = n->x+edge[0];
        int y = n->y+edge[1];
        if(x<0 || x>gridSize-1 || y<0 || y>gridSize-1)
          continue;
        if(isClosed(x,y))
          continue;

        if(x>gridSize/2 && y<x-gridSize/2)
          continue;

        if(y>gridSize/2 && x<y-gridSize/2)
          continue;

        Node* n2 = getNode(x,y);

        int g = n->g + pgm_read_byte( data + (y*gridSize + x) );

        if (n2->g > g && n2->inOpen){
          n2->g = g;
          n2->f = g+n2->h;
        }else if( n2->inOpen == false){
          n2->g = g;
          n2->f = g+n2->h;
          n2->inOpen = true;
        }
      }
    }
    if(graph.index>OPEN_SIZE){
      destination = true;
      Serial.print(n->x);
      Serial.print("--");
      Serial.println(n->y);
      Serial.println("-----graph out of bounds--------");
    }
    removeNode(n->x, n->y);
  }

  Serial.println("----------------------");
  Serial.println(graph.index);
  Serial.println(millis()-t);
}

void loop() {
  // put your main code here, to run repeatedly:
}