#include <SPI.h>
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>

#define SCREEN_WIDTH        128 // OLED display width, in pixels
#define SCREEN_HEIGHT        64 // OLED display height, in pixels
#define OBSTACLE_NUM 3
// Declaration for an SSD1306 display connected to I2C (SDA, SCL pins)
#define OLED_RESET            4 // Reset pin # (or -1 if sharing Arduino reset pin)
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

const byte buttonPins[] = {4, 2, 5}; // LEFT, UP, RIGHT, DOWN
int state = 0;

int midPoint = 30;
float rocketX = 0;
float rocketY = 30;
float linearRocketMoveX = 1;
float linearRocketMoveY = 0.0;
int amplitude2 = 12;
int amplitude3 = 20;
int flagTop = 0;
int flagFront = 0;
int flagBottom = 0;

int obstacle[OBSTACLE_NUM][2];

void checkCollisionFromTop(){
  for(int o = 0 ; o < OBSTACLE_NUM ; o++ ){
    for(int j = 0 ; j < 8 ; j++ ){
      if(((int)rocketY == obstacle[o][1] + 2 || (int)rocketY == obstacle[o][1] + 3 ||(int)rocketY == obstacle[o][1] + 4 ||(int)rocketY == obstacle[o][1] + 5) && ((int)rocketX + j >= obstacle[o][0] - 2 && (int)rocketX + j <= obstacle[o][0] + 2)){
        flagTop = 1;
        break;
      }
    }
  }
}
void checkCollisionFromBottom(){
  for(int o = 0 ; o < OBSTACLE_NUM ; o++ ){
      for(int j = 0 ; j < 8 ; j++ ){
        if(((int)rocketY + 2 == obstacle[o][1] || (int)rocketY + 2 == obstacle[o][1] - 1 ||(int)rocketY + 2 == obstacle[o][1] - 2 || (int)rocketY + 2 == obstacle[o][1] - 3 ||(int)rocketY + 2 == obstacle[o][1] - 4) && ((int)rocketX + j >= obstacle[o][0] - 2 && (int)rocketX + j <= obstacle[o][0] + 2)){
          flagBottom = 1;
          break;
        }
      }
    }
}
void checkCollisionFromFront(){
  for(int o = 0 ; o < OBSTACLE_NUM ; o++ ){
      for(int j = 0 ; j < 8 ; j++ ){
        if(((int)rocketY - 3 >= obstacle[o][1] && (int)rocketY + 5  <= obstacle[o][1]) && ((int)rocketX  >= obstacle[o][0] - 5 && (int)rocketX <= obstacle[o][0])){
          if(rocketY > obstacle[o][1]){
            flagFront = 2;
            break;
          }
          flagFront = 1;
          break;
        }
      }
    }
}


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

  if(!display.begin(SSD1306_SWITCHCAPVCC, 0x3D)) {
    Serial.println(F("SSD1306 allocation failed"));
    for(;;);
  }

  for (byte i = 0; i < 3; i++) {
    pinMode(buttonPins[i], INPUT_PULLUP);
  }

  randomSeed(analogRead(A0));
  for(int k = 0; k < OBSTACLE_NUM; k++){
    obstacle[k][0] = random(0, 127);
    obstacle[k][1] = random(0, 63);
  }
  display.clearDisplay();
  display.display();

}

void loop() {
  Serial.println(midPoint);
  if(state == 0){

    rocketX = 0;
    rocketY = 30;
    midPoint = 30;

    byte buttonPin = buttonPress();
    if (buttonPin != 25){
      if(buttonPin == 5){
        state = 1;
      }
      if(buttonPin == 2){
        state = 2;
      }
      if(buttonPin == 4){
        state = 3;
      }
    }
  }


  display.clearDisplay();
  display.fillRect((int)rocketX, (int)rocketY ,8 ,3 , SSD1306_WHITE);
  for( int f = 0; f < OBSTACLE_NUM; f++){
    display.drawRect(obstacle[f][0], obstacle[f][1] ,2 ,2 , SSD1306_WHITE);
  }
  display.display();

  checkCollisionFromTop();

  if(state == 1){
    rocketY += linearRocketMoveY;
  }
  if(state == 2){
    rocketY = midPoint + amplitude2*sin(rocketX/6);
    if(flagTop){
      midPoint++;
      midPoint++;
      midPoint++;
    }
    if(flagBottom){
      midPoint--;
      midPoint--;
      midPoint--;
    }
    if(flagFront == 1){
      midPoint--;
      midPoint--;
      midPoint--;
    }
    if(flagFront == 2){
      midPoint++;
      midPoint++;
      midPoint++;
    }
  }
  if(state == 3){
    rocketY = midPoint + amplitude3*sin(rocketX/11);
    if(flagTop){
      midPoint++;
      midPoint++;
      midPoint++;
    }
    if(flagBottom){
      Serial.println("FLAGBOTTOM!");
      midPoint--;
      midPoint--;
      midPoint--;
    }
    if(flagFront == 1){
      midPoint--;
      midPoint--;
      midPoint--;
    }
    if(flagFront == 2){
      midPoint++;
      midPoint++;
      midPoint++;
    }
  
  }
  Serial.println(midPoint);
  flagTop = 0;
  flagBottom = 0;
  flagFront = 0;
  rocketX += linearRocketMoveX;

  if (midPoint > 30){
    midPoint--;
  }
  if (midPoint < 30){
    midPoint++;
  }
  if(rocketX>=118){
    state = 0;
  }

  delay(10);
}

byte buttonPress() {
  for (byte i = 0; i < 3; i++) {
    byte buttonPin = buttonPins[i];
    if (digitalRead(buttonPin) == LOW) {
      return buttonPin;
    }
  }
  return 25;
}