/* Libraries ----------------------------------------------------------*/
#include "ArduinoGraphics.h"
#include "Arduino_LED_Matrix.h"
#include <Wire.h>
#include "NanoEdgeAI.h"
#include "knowledge.h"
#include <Scheduler.h>
/* Defines  ----------------------------------------------------------*/
/* Matrix part */
#define HEIGHT            8
#define WIDTH             12
#define PLAYER_X          10  //initial player position
#define PLAYER_Y          4
/* NEAI part */
#define SENSOR_SAMPLES	  512
#define AXIS              1

/* Prototypes ----------------------------------------------------------*/
void introduction_message(void);
void get_microphone_data(void);
void adapt_game_level(void);
void print_score(uint16_t game_score);
void reset_global_variables(void);

/* Objects  ----------------------------------------------------------*/
ArduinoLEDMatrix matrix;

/* Global variables ----------------------------------------------------------*/
int game_ongoing = 0;
static byte wall_move = false;
static int8_t player_move = 0;
static uint8_t player_x = PLAYER_X, player_y = PLAYER_Y;
static uint8_t wall_start_pix = 0, wall_pos_x = 0, wall_size = 3;
static uint16_t score = 0, neai_ptr = 0, time_to_wait = 50;
static float neai_buffer[SENSOR_SAMPLES * AXIS] = {0.0};

int const AMP_PIN = A0;       // Preamp output pin connected to A0

/* NanoEdgeAI variables part */
uint8_t neai_code = 0;
uint16_t id_class = 0; // Point to id class (see argument of neai_classification fct)
float output_class_buffer[CLASS_NUMBER]; // Buffer of class probabilities
const char *id2class[CLASS_NUMBER + 1] = { // Buffer for mapping class id to class name
  "unknown",
  "up",
  "down",
};

/* Declare matrix to display */
byte frame[8][12] = {
  { 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, 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 },
  { 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, 0, 0, 0, 0, 0, 0 }
};

/* Setup function ----------------------------------------------------------*/
void setup() {
  Serial.begin(115200);
  Scheduler.startLoop(loop2); //first loop is wall movement, second is player

  matrix.begin();
  introduction_message(); //display a message when starting

  /* Initialize NanoEdgeAI AI */
  neai_code = neai_classification_init(knowledge);
  if (neai_code != NEAI_OK) {
    Serial.print("Not supported board.\n");
  }
  else {
    game_ongoing = 1; //launch game if nanoedge working correctly
  }
}

/* Infinite loop ----------------------------------------------------------*/
void loop() {
  if (game_ongoing) {
    /* Clean the last column of the matrix */
    for (uint8_t y = 0; y < HEIGHT; y++) {
      frame[y][WIDTH - 1] = 0;
      frame[y][WIDTH - 2] = 0;
    }

    /* Set wall position on the matrix using random */
    wall_start_pix = random(0, 5);

    /* Move the wall through matrix */
    do {
      if (wall_move) {
        for (uint8_t y = 0; y < HEIGHT; y++) {
          frame[y][wall_pos_x] = (y >= wall_start_pix && y < wall_start_pix + wall_size) ?  0 : 1;
          if (wall_pos_x > 1) {
            frame[y][wall_pos_x - 2] = 0;
          }
        }
        wall_pos_x++;
      }
      wall_move = !wall_move;

      /* Update display */
      matrix.renderBitmap(frame, HEIGHT, WIDTH);

      /* Adapt level */
      adapt_game_level();

      /* Check if player touch the wall */
      if (frame[player_y][player_x] == frame[player_y][player_x - 1]) {
        game_ongoing = 0; //stop the game
        reset_global_variables();
        return;
      }
    } while (wall_pos_x < WIDTH);
    /* Increment score counter */
    score++;
    /* Reset wall position */
    wall_pos_x = 0;
  }
}


void loop2() {
  if (game_ongoing) {
    /* make a classification only if we detect a sound */
    if (analogRead(AMP_PIN) > 400) {
      get_microphone_data();
      neai_classification(neai_buffer, output_class_buffer, &id_class);
      /* Player next movement based on class detected */
      if (id_class == 1) {
        player_move = -1; //up
      }
      else if (id_class == 2) {
        player_move = 1; //down
      }
    }
    else {
      //We don't want to repeat the same movement until we detect something else
      id_class = 0;
      player_move = 0;
    }
    //move the player
    move_player();

    /* Clean neai buffer */
    memset(neai_buffer, 0.0, AXIS * SENSOR_SAMPLES * sizeof(float));
  }
  delay(1);
}

/* Functions declaration ----------------------------------------------------------*/
void introduction_message()
{
  matrix.beginDraw();
  matrix.stroke(0xFFFFFFFF);
  matrix.textScrollSpeed(50);

  // add the text
  const char text[] = " Flappy bird! ";
  matrix.textFont(Font_5x7);
  matrix.beginText(0, 1, 0xFFFFFF);
  matrix.println(text);
  matrix.endText(SCROLL_LEFT);

  matrix.endDraw();
}

void move_player() {
  /* Move the player and check if it stays in the screen */
  if (player_y + player_move >= 0 && player_y + player_move < HEIGHT) {
    /* Clear player last position */
    frame[player_y][player_x] = 0;
    /* Change player y coordinate */
    player_y += player_move;
  }
  /* Display player in matrix */
  frame[player_y][player_x] = 1;

  /* Update display */
  matrix.renderBitmap(frame, HEIGHT, WIDTH);
}

/* function to get a single downsampled sample from sensor 
We get every values but keep only one every eight values because the model was train with data like this.
It permits to listen for a longer period of time and get a better accuracy
*/
void get_microphone_data()
{
  static uint16_t temp = 0; //stock values
  int sub = 0; //increment to downsample
  //while the buffer is not full
  while (neai_ptr < SENSOR_SAMPLES) {
    //if it is the eighth value
    if (sub > 8) {
      /* Fill neai buffer with new accel data */
      neai_buffer[neai_ptr] = analogRead(AMP_PIN);
      /* Increment neai pointer */
      neai_ptr++;
      sub = 0; //reset increment
    }
    else {
      temp = analogRead(AMP_PIN);
    }
    sub ++;
  }
      for(uint16_t i = 0; i < SENSOR_SAMPLES; i++) {
       Serial.print(neai_buffer[i]);
       Serial.print(" ");
    }
    Serial.print("\n");
  neai_ptr = 0;
}

void adapt_game_level()
{
  /* Adapt speed & hole size */
  if (score < 5) {
    time_to_wait = 50;
  }
  else if (score >= 5 && score < 10) {
    time_to_wait = 40;
  }
  else if (score >= 10 && score < 15) {
    time_to_wait = 30;
  }
  else if (score >= 15 && score < 20) {
    wall_size = 2;
  }
  else {
    wall_size = 1;
  }
  delay(time_to_wait);
}

void print_score(uint16_t game_score)
{
  uint8_t text_pos_x = (game_score < 10) ? 5 : 3;
  matrix.clear();
  matrix.beginDraw();
  matrix.stroke(0xFFFFFFFF);
  char text[3];
  itoa(game_score, text, 10);
  matrix.textFont(Font_4x6);
  matrix.beginText(text_pos_x, 1, 0xFFFFFF);
  matrix.println(text);
  matrix.endText();
  matrix.endDraw();
}

void reset_global_variables()
{
  memset(frame, 0, WIDTH * HEIGHT * sizeof(byte));
  player_x = PLAYER_X;
  player_y = PLAYER_Y;
  print_score(score);
  /* Reset score after loosing the party */
  score = 0;
  /* Reset wall position */
  wall_pos_x = 0;
  /* Reset wall size */
  wall_size = 3;
  /* Reset delay */
  time_to_wait = 50;
  delay(1000);
  game_ongoing = 1;
}