#include <SPI.h>
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <iostream>
#include <vector>
#include <cstdlib>
#include <ctime>
#include <cmath>

#define SCREEN_WIDTH 128 // OLED display width, in pixels
#define SCREEN_HEIGHT 64 // OLED display height, in pixels

// Declaration for SSD1306 display connected using I2C
#define OLED_RESET     -1 // Reset pin
#define SCREEN_ADDRESS 0x3C
#define PIXEL_INTERVAL 5 

#define BUTTON_UP 34
#define BUTTON_MIDDLE 35
#define BUTTON_DOWN 32


Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

class Player {
  public:
      Player(int x_pos, int y_pos, int width, int height) : x_pos(x_pos), y_pos(y_pos), width(width), height(height)
      {
          std::cout << "player position initialized" << std::endl;
      }

      bool range_checker(int x, int y)
      {
        if(x <= 0 || x >= (width / 2)) return false;
        if(y <= 0 || y >= height) return false;
        return true;
      }
      
      void up()
      {
        std::cout << "MOVE UP" << std::endl;
        if(range_checker(x_pos, y_pos - 2)) y_pos -= 2;
      }

      void forward()
      {
        std::cout << "MOVE FORWARD" << std::endl;
        if(range_checker(x_pos + PIXEL_INTERVAL, y_pos)) x_pos += PIXEL_INTERVAL;
      }

      void back()
      {
        std::cout << "MOVE BACKWARD" << std::endl;
        if(range_checker(x_pos - PIXEL_INTERVAL, y_pos)) x_pos -= PIXEL_INTERVAL;
      }

      void down()
      {
        std::cout << "MOVE DOWN" << std::endl;
        if(range_checker(x_pos, y_pos + 2)) y_pos += 2;
      }


      std::pair<int, int> get_center_pos()
      {
        return {x_pos, y_pos};
      }

      std::vector<std::pair<int, int>> get_body()
      {
        return player_body;
      }

  private: 
      int x_pos;
      int y_pos;
      int width;
      int height;
      std::vector<std::pair<int, int>> player_body = {
        {-2, -2}, {-1, -2}, {0, -2}, {1, -2}, {2, -2},
        {-2, -1}, {-1, -1}, {0, -1}, {2, -1},
        {-2, 0}, {-1, 0}, {1, 0}, {2, 0},
        {-2, 1}, {-1, 1},
        {-2, -2}, {-1, 2}, {0, 2}, {1, 2}, {2, 2}
      };  
};

class Map {
  public:
      Map(int width, int height) : width(width), height(height)
      {
          game_map.resize(height, std::vector<int>(width, 0));
          std::cout << "map initialized" << std::endl;
      }

      void map_randomize() {
          auto range_tuple = range_randomizer();

          // replace bottom
          for (int bot = 0; bot < range_tuple.first; ++bot) {
              std::fill(game_map[bot].begin(), game_map[bot].end(), 1);
          }

          // replace top
          for (int up = range_tuple.second; up < height; ++up) {
              std::fill(game_map[up].begin(), game_map[up].end(), 1);
          }
      }

      // when player press RIGHT ARROW (moves the screen to the LEFT by 4 pixels)
      void add_new_frame(double probability = 0.5) {
          auto range_tuple = range_randomizer();

          for (int y = 0; y < height; ++y) {
              game_map[y].erase(game_map[y].begin(), game_map[y].begin() + PIXEL_INTERVAL);

              // add new frame at the end
              if (y <= range_tuple.first || y >= range_tuple.second) {
                  if ((double)rand() / RAND_MAX < probability) {
                      game_map[y].insert(game_map[y].end(), PIXEL_INTERVAL, 1);
                  } else {
                      game_map[y].insert(game_map[y].end(), PIXEL_INTERVAL, 0);
                  }
              } else {
                  game_map[y].insert(game_map[y].end(), PIXEL_INTERVAL, 0);
              }
          }
      }



      // update the map
      void update_map()
      {
        for (int y = 0; y < height; y++){
          for (int x = 0; x < width; x++){
            if (game_map[y][x] != 1)
              game_map[y][x] = 0;
          }
        }
      }

      // for setting objects on the map (for simplicity error won't be considered...)
      void set_object(int base_x, int base_y, std::vector<std::pair<int, int>> body){
        game_map[base_y].insert(game_map[base_y].begin() + base_x, 5);
        //std::cout << "BODY SIZE: " << body.size() << std::endl;
        for(int i = 0; i < body.size(); i++){
          //std::cout << "Iteration: " << i << std::endl;
          //game_map[base_y + body[i].second].insert(game_map[base_y + body[i].second].begin() + base_x + body[i].first, 5);
          game_map[base_y + body[i].second][base_x + body[i].first] = 5;
        }
      }

      void print_map() const {
          for (const auto& row : game_map) {
              for (int cell : row) {
                  std::cout << cell << " ";
              }
              std::cout << std::endl;
          }
      }

      const std::vector<std::vector<int>>& get_game_map() const {
          return game_map;
      }

  private:
      int width;
      int height;
      std::vector<std::vector<int>> game_map;

      std::pair<int, int> range_randomizer() {
          double r1 = static_cast<double>(rand()) / RAND_MAX * 0.35 + 0.1; // Range between 0.1 and 0.45
          int height_bottom_range = round(height * r1);
          int height_top_range = height - round(height * r1);
          return {height_bottom_range, height_top_range};
      }
};


// Setting up the MAP and the PLAYER
Map gameMap(SCREEN_WIDTH, SCREEN_HEIGHT);
Player player(50, 32, SCREEN_WIDTH, SCREEN_HEIGHT);
int switch_movement = 0;

void movement_selection(int curr)
{
  int curr_movement = curr % 20;
  if ((0 <= curr_movement && curr_movement < 5) || (15 <= curr_movement && curr_movement < 20)){
    player.forward();
    player.up();
  }
  else{
    player.forward();
    player.down();
  }
}



void setup()
{
  Serial.begin(9600);

  // Setting up the button
  pinMode(BUTTON_UP, INPUT_PULLUP);
  pinMode(BUTTON_MIDDLE, INPUT_PULLUP);
  pinMode(BUTTON_DOWN, INPUT_PULLUP);
  
  // initialize the OLED object
  if(!display.begin(SSD1306_SWITCHCAPVCC, SCREEN_ADDRESS)) {
    Serial.println(F("SSD1306 allocation failed"));
    for(;;); // Don't proceed, loop forever
  }

  // Setting up the initial map
  srand(static_cast<unsigned>(time(0))); 
  gameMap.map_randomize();

  // Setting up the initial player position
  gameMap.set_object(player.get_center_pos().first, player.get_center_pos().second, player.get_body());

  // Clear the buffer.
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(WHITE);
  display.setCursor(5,28);
  display.println("--SETUP COMPLETE--");
  std::cout << "--SETUP COMPLETE--" << std::endl;
  display.display();
  delay(2000);
  display.clearDisplay();
}

void loop() {
  display.clearDisplay();

  // User Input (button)
  std::cout << "UP: " << digitalRead(BUTTON_UP) << std::endl;
  if(digitalRead(BUTTON_UP) == 0){
    player.up();
  }

  std::cout << "MIDDLE: " << digitalRead(BUTTON_MIDDLE) << std::endl;
  if(digitalRead(BUTTON_MIDDLE) == 0){
    player.forward();
  }

  std::cout << "DOWN: " << digitalRead(BUTTON_DOWN) << std::endl;
  if(digitalRead(BUTTON_DOWN) == 0){
    player.down();
  }

  gameMap.set_object(player.get_center_pos().first, player.get_center_pos().second, player.get_body());
  // movement_selection(switch_movement);
  // switch_movement++;

  // Game background
  for (int y = 0; y < SCREEN_HEIGHT; ++y) {
    for (int x = 0; x < SCREEN_WIDTH; ++x) {
        display.drawPixel(x, y, gameMap.get_game_map()[y][x] ? WHITE : BLACK);
    }
  }

  Serial.print("Free Heap: ");
  Serial.println(esp_get_free_heap_size());
  gameMap.add_new_frame(0.3);
  display.display();

  gameMap.update_map();
}