Snake

#include <Snake.h>
#include <Adafruit_NeoPixel.h>

// ===============================
// ========== Constants ==========
// ===============================

#define IO_PIN      10
#define NUM_PIXEL   100
#define BRIGHTNESS  255
#define TIME_DELAY  200

#define MATRIX_SIZE   10
#define MATRIX_EMPTY  0
#define MATRIX_APPLE  1
#define MATRIX_SNAKE  2

#define DIRECTION_OBEN    0
#define DIRECTION_RECHTS  1
#define DIRECTION_UNTEN   2
#define DIRECTION_LINKS   3
#define DIRECTION_ERROR   4

#define RED   0x00FF0000
#define GREEN 0x0000FF00
#define BLUE  0x000000FF

// =========================================
// ========== Method deklarations ==========
// =========================================

void printNumber(const char* name, long value);
void printNumberln(const char* name, long value);

// =============================
// ========== Structs ==========
// =============================

struct Step {
  uint8_t direction;
  float distance;
};

struct PathNode {
  PathNode* parent = nullptr;
  uint8_t length = 0;
  uint8_t x: 4, y: 4;
};

struct CoordinateArray {
  CoordinateArray(Coordinate* coords, uint8_t length) {
    this->coords = coords;
    this->length = length;
  }

  Coordinate* coords;
  uint8_t length;
};



Snake snake;
Coordinate apple;
Step steps[4];
Coordinate directions[4];
uint8_t directionArray[4];

long periodEnd;

Adafruit_NeoPixel strip = Adafruit_NeoPixel(NUM_PIXEL, IO_PIN, NEO_GRBW + NEO_KHZ800);

// ====================================
// ========== Main-Functions ==========
// ====================================

void setup() {
  randomSeed(analogRead(0));
  Serial.begin(9600);
  while (!Serial) {
    ;
  }

  initializeDirections();
  initializeLedStrip();

  snake = Snake();

  createNewApple();
  apple.x = apple.y = 9;
  //ledTest();

  periodEnd = millis() + TIME_DELAY;
  renderToScreen();
  randomizeDirections();
}



void loop() {
  periodEnd = millis() + TIME_DELAY;

  Coordinate nextCoord = runDijkstra(snake[0]); //max 31millis

  if (nextCoord.y < snake[0].y) {
    moveSnake(DIRECTION_OBEN);
  } else if (nextCoord.y > snake[0].y) {
    moveSnake(DIRECTION_UNTEN);
  } else if (nextCoord.x > snake[0].x) {
    moveSnake(DIRECTION_RECHTS);
  } else if (nextCoord.x < snake[0].x) {
    moveSnake(DIRECTION_LINKS);
  }

  if (snakeAteApple()) {
    createNewApple();
    randomizeDirections();
  }

  while (periodEnd > millis()) {
    ; // Wait
  }

  renderToScreen();
}

/** Randomize directions */
void randomizeDirections(){
  uint8_t newNumber;

  for (uint8_t i = 0; i < 4; i++){
    directionArray[i] = 4;
  }

  for (uint8_t i = 0; i < 4; i++){
    do {
      newNumber = random(4);
    } while (directionAlreadyInUse(newNumber));

    directionArray[i] = newNumber;
  }
}

bool directionAlreadyInUse(uint8_t number){
  for (uint8_t i = 0; i < 4; i++){
    if (directionArray[i] == number){
      return true;
    }
  }

  return false;
}

/**
   Tests, if the snake ate the apple
*/
bool snakeAteApple() {
  if ((snake[0].x == apple.x) && (snake[0].y == apple.y)) {
    return true;
  } else {
    return false;
  }
}

/**
    Resets the game
*/
void restart() {
  snake.reset();
  createNewApple();
}

/**
   Moves the Snake to the new position
*/
void moveSnake(uint8_t direction) {
  switch (direction) {
    case DIRECTION_OBEN: snake.moveUp(); break;
    case DIRECTION_RECHTS: snake.moveRight(); break;
    case DIRECTION_UNTEN: snake.moveDown(); break;
    case DIRECTION_LINKS: snake.moveLeft(); break;
  }
}

// ==============================
// ========== RENDERER ==========
// ==============================

/**
   Renders the led strip
*/
void renderToScreen() {
  clearLedStrip();
  renderApple();
  renderSnake();
  strip.show();
}

/**
   Renders the apple to the led strip
*/
void renderApple() {
  uint8_t pos = calcLedPos(apple);
  strip.setPixelColor(pos, RED);
}

/**
   Rendes the snake to the led strip
*/
void renderSnake() {
  uint8_t pos;
  uint32_t snakeSegmentColor = strip.Color(128, 255, 0);

  pos = calcLedPos(snake[0]);
  strip.setPixelColor(pos, strip.Color(255, 255, 0));

  for (uint8_t i = 1; i < SNAKE_LENGTH; i++) {
    pos = calcLedPos(snake[i]);
    strip.setPixelColor(pos, snakeSegmentColor);
    snakeSegmentColor -= 0x200000;
  }
}

/**
   Calculates the position of a coordinate on the led strip
*/
uint8_t calcLedPos(Coordinate coord) {
  uint8_t pos = coord.y * MATRIX_SIZE;

  if ((coord.y % 2) == 0) {
    return pos + coord.x;
  } else {
    return pos + MATRIX_SIZE - coord.x - 1;
  }
}

/**
   Creates a new apple in the matrix
*/
void createNewApple() {
  int8_t x, y;

  do {
    x = random(MATRIX_SIZE);
    y = random(MATRIX_SIZE);
  } while (snake.coordIsTaken(x, y));

  apple.x = x;
  apple.y = y;
}


// ===========================
// ========== INITS ==========
// ===========================

/**
   Initializes the directions-array
*/
void initializeDirections() {
  directions[DIRECTION_OBEN] = Coordinate(0, -1);
  directions[DIRECTION_RECHTS] = Coordinate(1, 0);
  directions[DIRECTION_UNTEN] =  Coordinate(0, 1);
  directions[DIRECTION_LINKS] = Coordinate(-1, 0);
}

/**
   Initializes the led strip
*/
void initializeLedStrip() {
  strip.begin(); // This initializes the NeoPixel library.
  strip.setBrightness(BRIGHTNESS); // set brightness
  strip.show(); // Initialize all pixels to 'off'
}

/**
   Tests all leds by letting them blink in all 3 colors
*/
void ledTest() {
  uint8_t timeDelay = 20;

  for (uint8_t i = 0; i < NUM_PIXEL; i++) {
    clearLedStrip();
    strip.setPixelColor(i, RED);
    strip.show();
    delay(timeDelay);
  }

  for (uint8_t i = 0; i < NUM_PIXEL; i++) {
    clearLedStrip();
    strip.setPixelColor(i, GREEN);
    strip.show();
    delay(timeDelay);
  }

  for (uint8_t i = 0; i < NUM_PIXEL; i++) {
    clearLedStrip();
    strip.setPixelColor(i, BLUE);
    strip.show();
    delay(timeDelay);
  }
}

void clearLedStrip() {
  for (uint8_t i = 0; i < NUM_PIXEL; i++) {
    strip.setPixelColor(i, 0);
  }
}

// ==============================
// ========== DIJKSTRA ==========
// ==============================´

#define OPEN_LIST_MAX 30
#define ALL_LIST_MAX 100

// ArrayList - OpenList
PathNode* openList[OPEN_LIST_MAX];
uint8_t openList_pos;

// ArrayList - All
PathNode* allList[ALL_LIST_MAX];
uint8_t allList_pos;



uint8_t calcPos(uint8_t x, uint8_t y, uint8_t x_length) {
  return y * x_length + x + 1;
}

bool nodeAlreadyExists(uint8_t x, uint8_t y) {
  for (uint8_t i = 0; i < allList_pos; i++) {
    if ((allList[i]->x == x) && (allList[i]->y == y)) {
      return true;
    }
  }

  return false;
}

bool isWithinMatrix(PathNode* nextNode, Coordinate coord) {
  if (!((nextNode->x + coord.x >= 0) && (nextNode->x + coord.x < MATRIX_SIZE))) {
    return false;
  }


  if (!((nextNode->y + coord.y >= 0) && (nextNode->y + coord.y < MATRIX_SIZE))) {
    return false;
  }

  return true;
}

void testDirection(uint8_t nextDirection, PathNode* nextNode) {
  Coordinate coord = directions[nextDirection];

  if (isWithinMatrix(nextNode, coord) && (!snake.coordIsTaken(nextNode->x + coord.x, nextNode->y + coord.y)) && (!nodeAlreadyExists(nextNode->x + coord.x, nextNode->y + coord.y))) {
    PathNode* newNode = new PathNode;
    newNode->parent = nextNode;
    newNode->length = nextNode->length + 1;
    newNode->x = nextNode->x + coord.x;
    newNode->y = nextNode->y + coord.y;
    openList[openList_pos++] = newNode;
    allList[allList_pos++] = newNode;
  }
}

Coordinate runDijkstra(Coordinate start) {
  openList_pos = 0;
  allList_pos = 0;

  uint8_t nextNodeIndex = 0;
  PathNode* nextNode = nullptr;
  PathNode* originNode = nullptr;

  // Init
  originNode = nextNode = new PathNode;
  allList[allList_pos++] = nextNode;

  nextNode->length = 0;
  nextNode->x = start.x;
  nextNode->y = start.y;

  // Origin-Element abarbeiten
  openList[openList_pos++] = nextNode;

  uint8_t bestLength;
  uint8_t matrixFieldValue;
  while (true) {
    // Node mit kleinster Laenge suchen
    bestLength = 255;
    for (uint8_t i = 0; i < openList_pos; i++) {
      //printNode(openList[i]); // TODO
      if (openList[i]->length < bestLength) {
        nextNode = openList[i];
        bestLength = nextNode->length;
        nextNodeIndex = i;
      }
    }

    // Wenn diese Node Ziel ist, wird sie zurueck gegeben
    if ((nextNode->x == apple.x) && (nextNode->y == apple.y)) {
      uint8_t length = nextNode->length - 1;

      for (uint8_t i = 0; i < length; i++) {
        nextNode = nextNode->parent;
      }

      Coordinate coord(nextNode->x, nextNode->y);

      for (uint8_t i = 0; i < allList_pos; i++) {
        delete allList[i];
      }

      return coord;
    }

    // Directions absuchen
    for (uint8_t i = 0; i < 4; i++){
      testDirection(directionArray[i], nextNode);
    }

    // NextNode aus der Liste loeschen
    for (uint8_t i = nextNodeIndex; i < OPEN_LIST_MAX - 1; i++) {
      openList[i] = openList[i + 1];
    }
    openList_pos--;
  }
}

void printNumber(const char* name, long value) {
  Serial.print(name );
  Serial.print(value, DEC);
}

void printNumberln(const char* name, long value) {
  Serial.print(name );
  Serial.println(value, DEC);
}