#include "app.h"

#define MAP_MAX_X 5
#define MAP_MAX_Y 5
#define START_POINT 0
#define END_POINT 13
#define NEXT_X 5
#define NEXT_Y 1

Point Map[25];
int Directions[25][25] = {
  {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},
  {1,0,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,0,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,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,1,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
  {1,0,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,1,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0},
  {0,0,1,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0},
  {0,0,0,1,0,0,0,0,0,1,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0},
  {0,0,0,0,1,0,0,0,1,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0},
  {0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0},
  {0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0},
  {0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0},
  {0,0,0,0,0,0,0,0,1,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0},
  {0,0,0,0,0,0,0,0,0,1,0,0,0,1,0,0,0,0,0,1,0,0,0,0,0},
  {0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,1,0,0,0,0},
  {0,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,1,0,0,0,0,0,0,0},
  {0,0,0,0,0,0,0,0,0,0,0,0,1,0,0,0,1,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,1,0,1,0,0,0,0,0},
  {0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,0,0,0,1,0,0,0,0,0,1},
  {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,0,0,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,0,0,1,0,1,0,0},
  {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,0,0,0,1,0,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,0,1},
  {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,0,0,0,1,0}
};

void setup() {
  Serial.begin(9600);
  initMap();
  pathToPoint();
}

void loop() {
  /*
  int i = random(25);
  Serial.print("Case ");
  Serial.print(i);
  Serial.print(", y: ");
  Serial.print(Map[i].y);
  Serial.print(", x: ");
  Serial.println(Map[i].x);*/
  delay(1000);
}

void pathToPoint() {
  // Récupère les coordonnées des points importants
  Point START = Map[START_POINT];
  Point END = Map[END_POINT];
  Point ACTUAL = START;

  Serial.print("Start: ");
  Serial.print(START.id);
  Serial.print(", y");
  Serial.print(START.y);
  Serial.print(", x");
  Serial.println(START.x);

  Serial.print("End: ");
  Serial.print(END.id);
  Serial.print(", y");
  Serial.print(END.y);
  Serial.print(", x");
  Serial.println(END.x);

  Serial.print("Actual: ");
  Serial.print(END.id);
  Serial.print(", y");
  Serial.print(END.y);
  Serial.print(", x");
  Serial.println(END.x);

  while (ACTUAL.x != END.x && ACTUAL.y != END.y) {
    searchEnd(Directions[START.id]);
  }
  /*Serial.print("Point: y ");
  Serial.print(END.y);
  Serial.print(", x ");
  Serial.println(END.x);
  Serial.print("Direction Y: ");
  Serial.println(YPos);
  Serial.print("Direction X: ");
  Serial.println(XPos);*/
}

void searchEnd(int *arr) {
  for(int i = 0; i < sizeof(*arr) / sizeof(int); i++)
    {
      Serial.println(i);

    }
}

/**
 * Initialise la map
 */
void initMap() {
  // Auto-génération des index de chaque point
  int x = 0;
  int y = 0;
  int id = 1;
  for(int i = 0; i < sizeof(Map) / sizeof(Point); i++)
  {
    Map[i].y = y;
    Map[i].x = x;
    Map[i].id = id;
    y++;
    if (y == MAP_MAX_Y) { y = 0; x += 1; };
    id++;
  };

}