#include <TinyWireM.h>
//#include <Tiny4kOLED.h>
#include <ssd1306xled.h>
#include <EEPROM.h>

#define EEPROM_SCORE_ADDRESS 0

#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64

//#define BUTTON_SCROLL_PIN PB3
//#define BUTTON_SELECT_PIN PB5
#define BUZZER_PIN PB1

const int mpuAddress = 0x68;
int mapGyro;

const int selectVal = 520; //change the value according to the actual button value
const int scrollVal = 693; //change the value according to the actual button value
const int buttonTolerance = 10; //change the value tolerance
int analogValue;

const char * menuItems[] = {
  "Regles du jeu  ",
  "Choisir un pseudo  ",
  "Commencer le jeu  "
};

const int numMenuItems = sizeof(menuItems) / sizeof(menuItems[0]);
int currentItem = 0;

const unsigned char car_bitmap_2[] PROGMEM = {
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xf8, 0xfe, 0xbe, 0xff, 0xff, 0xcf, 0xcf, 0xff, 0xff, 0xbe, 
  0xfe, 0xf8, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x04, 0xff, 
  0xff, 0x0f, 0x0f, 0x0f, 0x0f, 0x0f, 0x0f, 0xff, 0xff, 0x04, 0x08, 0x00, 0x00, 0x00, 0x00, 0x00, 
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x1f, 0x7f, 0x7f, 0xfc, 0xbc, 0x7c, 0x7c, 0xbc, 0xfc, 0x7f, 
  0x7f, 0x1f, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00
};

const unsigned char car_clear[] PROGMEM = {
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00
};

int score = 0;
int i_score = 0;
int hi_score = 0;
int speedIncreaseInterval = 5000;
unsigned long lastSpeedIncreaseTime = 0;
int roadLineOffset = 0;
int carX, carY;
int carWidth = 24, carHeight = 24;
int moveSpeed = 2;
const int numObstacles = 4;
int obstacleX[numObstacles], obstacleY[numObstacles];
int obstacleSpeed = 1;
bool gameOver = false;
bool gameStarted = false;

void resetObstacles() {
  int roadWidth = SCREEN_WIDTH / 4;
  int freeRoad = random(0, 4);

  for (int i = 0; i < numObstacles; i++) {
    if (i != freeRoad) {
      obstacleX[i] = i * roadWidth + (roadWidth - carWidth) / 2;
      obstacleY[i] = -random(10, 150);
    } else {
      obstacleY[i] = -200;
    }
  }
}

void chargerJeu() {
  //oled.clear();
  SSD1306.ssd1306_fillscreen(0);

  carX = (SCREEN_WIDTH - carWidth) / 2;
  carY = SCREEN_HEIGHT - carHeight - 5;

  resetObstacles();
  lastSpeedIncreaseTime = millis();
}

void drawObstacles() {
  for (int i = 0; i < numObstacles; i++) {
    if (obstacleY[i] > -carHeight && obstacleY[i] < SCREEN_HEIGHT) {
      drawCar(obstacleX[i], obstacleY[i]);
    }
    else{
      //oled.setCursor(0,7);
      //oled.fillToEOL(0x00);
      //oled.bitmap(obstacleX[i], 1, obstacleX[i]+carWidth, 2, car_clear);
      SSD1306.ssd1306_draw_bmp(obstacleX[i], 1, obstacleX[i]+carWidth, 2, car_clear);
      //oled.bitmap(obstacleX[i], 7, obstacleX[i]+carWidth, 8, car_clear);
      SSD1306.ssd1306_draw_bmp(obstacleX[i], 7, obstacleX[i]+carWidth, 8, car_clear);
    }
  }
}

void drawCar(int x, int y) {
  //oled.bitmap(x, y/8, x+carWidth, (y+carHeight)/8, car_bitmap_2);
  if (y % SCREEN_HEIGHT >= 48 && y % SCREEN_HEIGHT < 56){
    SSD1306.ssd1306_draw_bmp(x, y/8, x+carWidth, (y+carHeight-8)/8, car_bitmap_2);
  }
  else if (y % SCREEN_HEIGHT >= 56 && y % SCREEN_HEIGHT < SCREEN_HEIGHT){
    SSD1306.ssd1306_draw_bmp(x, y/8, x+carWidth, (y+carHeight-16)/8, car_bitmap_2);
  }
  else{
    SSD1306.ssd1306_draw_bmp(x, y/8, x+carWidth, (y+carHeight)/8, car_bitmap_2);
  }
  //oled.bitmap(x, y/8 - 1, x+carWidth, y/8, car_clear);
  SSD1306.ssd1306_draw_bmp(x, y/8 - 1, x+carWidth, y/8, car_clear);
  //oled.bitmap(x, (y+carHeight)/8, x+carWidth, (y+carHeight)/8 + 1, car_clear);
}

const uint8_t line7 [] PROGMEM = { B11110000 };
const uint8_t line6 [] PROGMEM = { B01111000 };
const uint8_t line5 [] PROGMEM = { B00111100 };
const uint8_t line4 [] PROGMEM = { B00001111 };
const uint8_t line3 [] PROGMEM = { B10000111 };
const uint8_t line2 [] PROGMEM = { B11000011 };
const uint8_t line1 [] PROGMEM = { B11100001 };

const unsigned char* line_anim_array[7] = {
  line1,
  line2,
  line3,
  line4,
  line5,
  line6,
  line7
};

byte line_anim_frame = 0;

void drawRoadLines() {
  for(int a = 1; a < 4; a++) {
    for(int i = 0; i < 8; i++) {
      //oled.bitmap(32*a, i, 32*a+1, i+1, line_anim_array[line_anim_frame]);
      SSD1306.ssd1306_draw_bmp(32*a, i, 32*a+1, i+1, line_anim_array[line_anim_frame]);
    }
  }

  line_anim_frame = (line_anim_frame + 1) % 7;
}

void updateObstacles() {
  for (int i = 0; i < numObstacles; i++) {
    obstacleY[i] += obstacleSpeed;

    if (obstacleY[i] > SCREEN_HEIGHT) {
      obstacleY[i] = -random(100, 200);
      score += 100;
      //oled.clear();
    }
  }
}

void drawScore() {
  //oled.setFont(FONT6X8);
  //oled.setCursor(0, 0);
  //oled.print("Score: ");
  //oled.print(score);

  SSD1306.ssd1306_setpos(0, 0);
  SSD1306.ssd1306_string_font6x8("Score: ");
  String score_string = String(score);
  char* score_char = score_string.c_str();
  SSD1306.ssd1306_string_font6x8(score_char);
}

void checkCollision() {
  int marginX = 7;
  int marginY = 2;

  for (int i = 0; i < numObstacles; i++) {
    int carLeft = carX + marginX;
    int carRight = carX + carWidth - marginX;
    int carTop = carY + marginY;
    int carBottom = carY + carHeight - marginY;

    int obstacleLeft = obstacleX[i] + marginX;
    int obstacleRight = obstacleX[i] + carWidth - marginX;
    int obstacleTop = obstacleY[i] + marginY;
    int obstacleBottom = obstacleY[i] + carHeight - marginY;

    if (carRight > obstacleLeft && carLeft < obstacleRight &&
      carBottom > obstacleTop && carTop < obstacleBottom) {
      gameOver = true;
      if(score > hi_score){
        EEPROM.write(EEPROM_SCORE_ADDRESS, score);
        hi_score = score;
      }
      digitalWrite(BUZZER_PIN, HIGH);
      delay(100);
      break;
    }
  }
}

/*
void afficherMenu() {
  oled.setFont(FONT6X8);
  for (int i = 0; i < 8; i++) {
    if (i < numMenuItems){
      oled.setCursor(0, i);
      if (i == currentItem) {
        oled.print(F("> "));
      }
      oled.print(menuItems[i]);
    }
    else{
      oled.setCursor(0, i);
      oled.fillToEOL(0x00);
    }
  }
  //oled.on();
}
*/
void afficherMenu() {
  //SSD1306.ssd1306_fillscreen(0);
  for (int i = 0; i < numMenuItems; i++) {
    SSD1306.ssd1306_setpos(0, i * 10);
    if (i == currentItem) {
      SSD1306.ssd1306_string_font6x8("> ");
    }
    SSD1306.ssd1306_string_font6x8(menuItems[i]);
  }
}

void gererSelectionMenu() {
  //if (digitalRead(BUTTON_SCROLL_PIN) == LOW) {
  if (analogRead(PB3) >= scrollVal - buttonTolerance && analogRead(PB3) <= scrollVal + buttonTolerance){
    currentItem = (currentItem + 1) % numMenuItems;
    delay(100);
  }
  //if (digitalRead(BUTTON_SELECT_PIN) == LOW) {
  if (analogRead(PB3) >= selectVal - buttonTolerance && analogRead(PB3) <= selectVal + buttonTolerance){
    if (currentItem == 2) {
      gameStarted = true;
      chargerJeu();
      SSD1306.ssd1306_fillscreen(0);
    }
    delay(100);
  }
}

void afficherGameOver() {
  digitalWrite(BUZZER_PIN, LOW);
  //oled.clear();
  SSD1306.ssd1306_fillscreen(0);
  //oled.setFont(FONT8X16);
  //oled.setCursor(0, 1);
  //oled.print(F("   GAME  OVER   "));
  SSD1306.ssd1306_string_font6x8("GAME OVER");
  //oled.setFont(FONT6X8);
  //oled.setCursor(0, 4);
  //oled.print(F("SCORE : "));
  //oled.print(score);
  //oled.print(F("            "));
  SSD1306.ssd1306_setpos(0, 10);
  SSD1306.ssd1306_string_font6x8("SCORE : ");
  String score_string = String(score);
  char score_char[score_string.length() + 1];
  strcpy(score_char, score_string.c_str());
  SSD1306.ssd1306_string_font6x8(score_char);
  //oled.setCursor(0, 5);
  //oled.print(F("HI SCORE : "));
  //oled.print(hi_score);
  //oled.print(F("            "));
  SSD1306.ssd1306_setpos(0, 20);
  SSD1306.ssd1306_string_font6x8("HI SCORE : ");
  String hi_score_string = String(hi_score);
  char hi_score_char[hi_score_string.length() + 1];
  strcpy(hi_score_char, hi_score_string.c_str());
  SSD1306.ssd1306_string_font6x8(hi_score_char);
  /*
  for (int i = 6; i < 8; i++){
    oled.setCursor(0, i);
    oled.fillToEOL(0x00);
  }
  */
  //oled.on();
  delay(2000);

  //while (digitalRead(BUTTON_SELECT_PIN) == LOW || digitalRead(BUTTON_SCROLL_PIN) == LOW) {
  while (analogRead(PB3) >= selectVal - buttonTolerance){
    delay(10);
  }

  gameOver = false;
  gameStarted = false;
  currentItem = 0;
  score = 0;
  obstacleSpeed = 1;
  resetObstacles();
  // oled.clear();
}

void mpuBegin(){
  TinyWireM.begin();
  TinyWireM.beginTransmission(mpuAddress);
  TinyWireM.write(mpuAddress);
  TinyWireM.write(0);
  auto error = TinyWireM.endTransmission();
}

void mpuMeasure(){
  TinyWireM.beginTransmission(mpuAddress);
  TinyWireM.write(0x3B);                   // Starting with register 0x3B (ACCEL_XOUT_H)
  TinyWireM.endTransmission(false);

  int16_t GyX,AcX, GyY, GyZ, AcY, AcZ, Tmp;

  TinyWireM.requestFrom( mpuAddress, 14);   // request a total of 14 bytes
  AcX = TinyWireM.read()<<8 | TinyWireM.read();  // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)    
  AcY = TinyWireM.read()<<8 | TinyWireM.read();  // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
  AcZ = TinyWireM.read()<<8 | TinyWireM.read();  // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
  Tmp = TinyWireM.read()<<8 | TinyWireM.read();  // 0x41 (TEMP_OUT_H)   & 0x42 (TEMP_OUT_L)
  GyX = TinyWireM.read()<<8 | TinyWireM.read();  // 0x43 (GYRO_XOUT_H)  & 0x44 (GYRO_XOUT_L)
  GyY = TinyWireM.read()<<8 | TinyWireM.read();  // 0x45 (GYRO_YOUT_H)  & 0x46 (GYRO_YOUT_L)
  GyZ = TinyWireM.read()<<8 | TinyWireM.read();  // 0x47 (GYRO_ZOUT_H)  & 0x48 (GYRO_ZOUT_L)
  
  mapGyro = constrain(map(GyX, -32768, 32767, 0, 104), 0, 104);
  //mapGyro = mapconstrain(map(AcX, -32768, 32767, 0, 100), 0, 100); //use this line if in the real device has different behaviour
}

void setup() {
  hi_score = EEPROM.read(EEPROM_SCORE_ADDRESS);
  //pinMode(BUTTON_SCROLL_PIN, INPUT_PULLUP);
  //pinMode(BUTTON_SELECT_PIN, INPUT_PULLUP);
  pinMode(BUZZER_PIN, OUTPUT);
  digitalWrite(BUZZER_PIN, LOW);
  
  mpuBegin();

  //oled.begin(SCREEN_WIDTH, SCREEN_HEIGHT, sizeof(tiny4koled_init_128x64br), tiny4koled_init_128x64br);
  SSD1306.ssd1306_init();
  //oled.clear();
  SSD1306.ssd1306_fillscreen(0);
  //oled.on();
}

void loop() {
  digitalWrite(BUZZER_PIN, LOW);
  if (!gameStarted) {
    afficherMenu();
    gererSelectionMenu();
  } else {
    if (!gameOver) {
      drawRoadLines();
      drawScore();
      
      /*
      if (digitalRead(BUTTON_LEFT_PIN) == LOW && carX > 0) {
        carX -= moveSpeed;
      }
      if (digitalRead(BUTTON_RIGHT_PIN) == LOW && carX < SCREEN_WIDTH - carWidth) {
        carX += moveSpeed;
      }
      */
      mpuMeasure();
      carX = mapGyro;

      drawCar(carX, carY);
      updateObstacles();
      drawObstacles();
      checkCollision();

      //oled.on();
    } else {
      afficherGameOver();
    }
  }
}