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

#define DR              0.0174533

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

// Declaration for an SSD1306 display connected to I2C (SDA, SCL pins)
#define OLED_RESET      4 // Reset pin # (or -1 if sharing Arduino reset pin)
#define pin_IR      2 // Pin for IR
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

#define LOGO_HEIGHT     16  
#define LOGO_WIDTH      16

#define FONT_HEIGHT     8

static const unsigned char PROGMEM logo_bmp[] = {
    B0000000,  B11000000,
    B0000001,  B11000000,
    B00000001, B11000000,
    B00000011, B11100000,
    B11110011, B11100000,
    B11111110, B11111000,
    B01111110, B11111111,
    B00110011, B10011111,
    B00011111, B11111100,
    B00001101, B01110000,
    B00011011, B10100000,
    B00111111, B11100000,
    B00111111, B11110000,
    B01111100, B11110000,
    B01110000, B01110000,
    B00000000, B00110000 
};

//Hand bitmap
unsigned char hand_w = 16, hand_h = 16;

static const unsigned char PROGMEM hand_spr[] = {
    B00000000, B00000000,
    B11111011, B00000000,
    B11111101, B10111000, 
    B10111110, B11011100,
    B00111110, B11111100,
    B00011111, B11111110,
    B00011111, B11111110,
    B11001111, B11111110,
    B11111111, B11111110, 
    B11111111, B11111111,
    B11111111, B11111111,
    B11111111, B11111111, 
    B00111111, B11111111, 
    B00011111, B11111111,
    B00001111, B11111111, 
    B00000111, B11111111, 
};

//Logo bitmap
unsigned char icon_w = 32, icon_h = 16;

static const unsigned char PROGMEM icon_spr[] = {
    B11111110, B00111110, B01111101, B11101111,
    B01111111, B01111111, B11111110, B11111110,
    B01111111, B01111111, B11111110, B11111110,
    B01110111, B01110111, B11101110, B11111110,
    B01110111, B01110111, B11101110, B11111110,
    B01110110, B01110111, B11101110, B11011110,
    B01111110, B01110111, B11101110, B11011110,
    B01111111, B01110111, B11101110, B11011110,
    B01110111, B01110111, B11101110, B11011110,
    B01110111, B01110111, B11101110, B11010110,
    B01110111, B01111111, B11111110, B11010110,
    B01110111, B01111111, B11111110, B11010110,
    B01110111, B01111111, B11111110, B11010110,
    B01110100, B00111110, B01111100, B01000110,
    B01100000, B00000000, B00000000, B00000110,
    B01000000, B00000000, B00000000, B00000010,
};

//Button Inputs
const int LEFT_BUTTON   = 3;
const int RIGHT_BUTTON  = 5; //Can't use 4 becausethis is the reset for the screen
const int UP_BUTTON     = 6;
const int DOWN_BUTTON   = 7;
const int SWITCH_BUTTON = 8;
const int PIEZO         = 9;

bool left_state    = 0;
bool right_state   = 0;
bool up_state      = 0;
bool down_state    = 0;
bool switch_state  = 0;

//Movement Variables
int xspd = 0, yspd = 0;
float move_x = 0, move_y = 0, cur_velocity = 0;

//Player Variables
int move_spd = 1;
int player_health = 100, player_armour = 0; //Stats

//UI / Display Variables
float hand_x_offset = 0, hand_y_offset = 0, move_step = 0;

//Camera Variables
float cam_x, cam_y, cam_z, cam_delta_x, cam_delta_y, cam_angle, cam_angle_to;

//Game vars
int score = 0;

//Map Array
int map_x=8, map_y=8, map_size=map_x*map_y, map_offset = 4, map_display = false;
int map_grid[] = {
  1,1,1,1,1,1,1,1,
  1,0,0,1,0,0,0,1,
  1,0,0,0,0,0,0,2,
  1,1,0,1,1,0,0,1,
  1,0,0,1,1,0,0,1,
  1,0,0,0,0,0,0,1,
  1,0,0,1,0,0,0,1,
  1,2,1,1,1,1,1,1
};

//Clock Vars
int clock_time = 0;
int clock_seconds = 0;
int clock_minutes = 0;
int clock_hours = 0;

float seconds_progress = 0;

//Connect the IR remote and toggle the game to off
int program_on = true;

//GAMEPLAY FUNCTIONALITY
void setup() {
  Serial.begin(9600);
  while(!Serial);
  Serial.begin(9600);
  Serial.println(F("Startup"));

  //Input / Output setup
  pinMode(LEFT_BUTTON,  INPUT);
  pinMode(RIGHT_BUTTON, INPUT);
  pinMode(UP_BUTTON,    INPUT);
  pinMode(DOWN_BUTTON,  INPUT);
  pinMode(SWITCH_BUTTON,INPUT);
  pinMode(PIEZO,        OUTPUT);

  //Camera Position
  cam_x = 10;
  cam_y = 10;
  cam_z = 0;
  cam_angle = 0;
  cam_angle_to = 0;
  cam_delta_x = cos(cam_angle);
  cam_delta_y = sin(cam_angle);
  
  //Setup text functionality
  display.setTextSize(1);      // Normal 1:1 pixel scale
  display.setTextColor(SSD1306_WHITE); // Draw white text
  display.setCursor(0, 0);     // Start at top-left corner
  display.cp437(true);         // Use full 256 char 'Code Page 437' font

  // SSD1306_SWITCHCAPVCC = generate display voltage from 3.3V internally
  if(!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) { // Address 0x3D for 128x64
    // ^^^^^^^^^^^^^ FOR SOME REASON THE ONLINE NEEDS 0x3D AND THE ONE WITH ARDUINO NEEDS 0x3C.

    Serial.println(F("SSD1306 allocation failed"));
    for(;;); // Don't proceed, loop forever
  }

  //Clear SSD1306 display
  display.clearDisplay();

  //Startup Music
  int note_time = 50;
  int delay_time = 75;

  //FIRST
  tone(PIEZO, 500);
  delay(note_time); 
  
  noTone(PIEZO);     
  delay(delay_time);       

  tone(PIEZO, 500);
  delay(note_time);
  
  noTone(PIEZO);     
  delay(delay_time);  

  tone(PIEZO, 1000);
  delay(note_time);
  
  noTone(PIEZO);     
  delay(delay_time);  

  //SEOCND
  tone(PIEZO, 500);
  delay(note_time); 
  
  noTone(PIEZO);     
  delay(delay_time);       

  tone(PIEZO, 500);
  delay(note_time);
  
  noTone(PIEZO);     
  delay(delay_time);  

  tone(PIEZO, 900);
  delay(note_time);
  
  noTone(PIEZO);     
  delay(delay_time);  

  //RHIRTH
  tone(PIEZO, 500);
  delay(note_time); 
  
  noTone(PIEZO);     
  delay(delay_time);       

  tone(PIEZO, 500);
  delay(note_time);
  
  noTone(PIEZO);     
  delay(delay_time);  

  tone(PIEZO, 800);
  delay(note_time);
  
  noTone(PIEZO);     
  delay(delay_time);  

  //RHIRTH
  tone(PIEZO, 500);
  delay(note_time); 
  
  noTone(PIEZO);     
  delay(delay_time);       

  tone(PIEZO, 500);
  delay(note_time);
  
  noTone(PIEZO);     
  delay(delay_time);  

  tone(PIEZO, 700);
  delay(note_time);
  
  noTone(PIEZO);     
  delay(delay_time);  

  tone(PIEZO, 500);
  delay(note_time);

  noTone(PIEZO);     
  delay(delay_time);  

  tone(PIEZO, 500);
  delay(note_time);

  noTone(PIEZO);     
  delay(delay_time);  

  tone(PIEZO, 450);
  delay(note_time*4);

  noTone(PIEZO);     
  delay(1000);  

  //Display Logo
  display.clearDisplay();
  display.drawBitmap((SCREEN_WIDTH-icon_w)/2, (SCREEN_HEIGHT-icon_h)/2, icon_spr, icon_w, icon_h, 1);
  
  // Show initial display buffer contents on the screen --
  // the library initializes this with an Adafruit splash screen.
  display.display();
  delay(1000); // Pause for 2 seconds
}

void loop() {
  //Get Inputs from buttons
  left_state   = digitalRead(LEFT_BUTTON);
  right_state  = digitalRead(RIGHT_BUTTON);
  up_state     = digitalRead(UP_BUTTON);
  down_state   = digitalRead(DOWN_BUTTON);
  switch_state = digitalRead(SWITCH_BUTTON);
  
  //Toggle between clock and game
  if(!program_on){
    display.display();
    display.clearDisplay();
    
    display.setTextSize(1);      // Normal 1:1 pixel scale
    display.setTextColor(SSD1306_WHITE); // Draw white text
    display.setCursor(8, 8);     // Start at top-left corner
    display.cp437(true);         // Use full 256 char 'Code Page 437' font

    //Draw the screen
    display_time(clock_minutes, clock_seconds, seconds_progress);

    //Tick the time
    delay(70);
    seconds_progress += 0.1;
    if(seconds_progress >= 1) {
      seconds_progress = 0;
      clock_seconds += 1;

      if(clock_seconds >= 60){
        clock_minutes += 1;
        clock_seconds = 0;
      }

      if(clock_minutes >= 60*24){
        clock_minutes = 0;
      }
    }

    //Check Inputs for clock
    if(left_state == HIGH) { //Left button - Increase minute
      clock_minutes += 1;
      clock_seconds = 0;

      if(clock_minutes >= 60*24){
        clock_minutes = 0;
      }
    }
    
    if(up_state == HIGH) { //Middle button - Decrease minute
      clock_minutes -= 1;
      clock_seconds = 0;

      if(clock_minutes < 0){
        clock_minutes = 60*24 - 1;
      }
    }

    if(right_state == HIGH) { //Right button - Increase minutes by 60 (1 hour)
      clock_minutes += 60;
      clock_seconds = 0;

      if(clock_minutes >= 60*24){
        clock_minutes -= 60*24;
      }
    }
  }else{
    //Refresh rate - Update Display
    display.display();
    display.clearDisplay();
    delay(1);
  
    //Check Inputs and apply movement
    if(left_state == HIGH) { //Move Left
      cam_angle_to -= 0.2;
      hand_x_offset += 3;
    }
    
    if(right_state == HIGH) { //Move Right
      cam_angle_to += 0.2;
      hand_x_offset -= 3;
    }
    
    //Smooth the angle
    cam_angle = lerp(cam_angle, cam_angle_to, 0.5);
    
    //Update deltas
    cam_delta_x = cos(cam_angle)*move_spd;
    cam_delta_y = sin(cam_angle)*move_spd;

    //Movement speed and "real" direction
    xspd = 0;
    yspd = ((up_state==HIGH)-(down_state==HIGH));

    //Get movement speed
    cur_velocity = lerp(cur_velocity, yspd, 0.6);
    move_x = cam_delta_x*cur_velocity;
    move_y = cam_delta_y*cur_velocity;
    
    //Collisions
    int ray_result = cast_ray(cam_x, cam_y, cam_x+(cam_delta_x)*3, cam_y+(cam_delta_y)*3);
    if(ray_result == 1){
      move_x = 0;
      move_y = 0;
    }else if(ray_result == 2){ //END
      int note_time = 50;
      int delay_time = 75;
  
      //Play a tune when the player reaches an exit
      tone(PIEZO, 500);
      delay(note_time); 
      
      noTone(PIEZO);     
      delay(delay_time);       

      tone(PIEZO, 500);
      delay(note_time);
      
      noTone(PIEZO);     
      delay(delay_time);  

      tone(PIEZO, 700);
      delay(note_time);
      
      noTone(PIEZO);     
      delay(delay_time);  

      tone(PIEZO, 500);
      delay(note_time);

      noTone(PIEZO);     
      delay(delay_time);  

      tone(PIEZO, 500);
      delay(note_time);

      noTone(PIEZO);     
      delay(delay_time);  

      tone(PIEZO, 450);
      delay(note_time*4);

      noTone(PIEZO);     
      delay(500);  
      
      //Reset the position and increase score
      cam_x = 10;
      cam_y = 10;
      score += 1;
    }else{
      //Move the canera
      cam_x += move_x;
      cam_y += move_y;
    }

    //Render the display
    render_display();
  }
}

//Render Function
void render_display() {
  //3d view
  cast_rays_3d();

  //Hand and score
  draw_ui();
}

//FUNCTION STUFF
void draw_map_2d() {
  //Draw a birds-eye view of the map
  for(int i = 0; i < map_x; i++){
    for(int j = 0; j < map_y; j++){
      if(map_grid[j*map_x + i] == 1) display.fillRect(i*map_offset, j*map_offset, map_offset, map_offset, SSD1306_WHITE);
    }
  }
}

void draw_player() {
  //Draw a player on the birds-eye map
  display.drawPixel(cam_x, cam_y, SSD1306_WHITE);
  display.drawLine(cam_x, cam_y, cam_x+cam_delta_x*4, cam_y+cam_delta_y*4, SSD1306_WHITE);
}

void draw_ui() {
  //Clear behind
  display.fillRect(0, SCREEN_HEIGHT-10, SCREEN_WIDTH, 10, SSD1306_BLACK);
  
  //Control hand offsets and head offset
  if(yspd != 0){
    //Player is moving
    hand_x_offset = lerp(hand_x_offset, sin(move_step) * 8, 0.3);
    hand_y_offset = lerp(hand_y_offset, cos(move_step*2) * 4, 0.3);
    cam_z = lerp(cam_z, cos(move_step*2) * 4, 0.3);

    //cam_z = sin(millis()*0.001)*10;
    
    //Used for headbob
    move_step += 0.6*yspd;
  }else{
    //Reset the hand on the UI and the headbob step
    hand_x_offset = lerp(hand_x_offset, 0, 0.3);
    hand_y_offset = lerp(hand_y_offset, 0, 0.3);
    cam_z = lerp(cam_z, 0, 0.3);
    move_step = 0;
  }
  
  //Draw the score
  display.setCursor(2, SCREEN_HEIGHT-8);
  display.print("SCR:");
  display.print(score);

  //Draw the hand 
  display.drawBitmap(round(hand_x_offset) + (SCREEN_WIDTH-hand_w)/2, round(hand_y_offset) + SCREEN_HEIGHT-(hand_h/2), hand_spr, hand_w, hand_h, 1);
}

int cast_ray(int x, int y, int xto, int yto) {
  //Casts a ray from [x, y] to [xto, yto]. Used for collision detection
  float ray_x, ray_y, x_off, y_off, distance;
  int r_map_x, r_map_y, r_map_pos;
  
  //Translate a "world position" to a grid position.
  r_map_x = (xto)/map_offset;
  r_map_y = (yto)/map_offset;
  r_map_pos = r_map_y*map_x + r_map_x;
  if(r_map_pos < map_size && map_grid[r_map_pos] != 0){
      //Return the collided grid's ID
      int ret = map_grid[r_map_pos];
      return(ret);
  }
  //No collision detected
  return false;
}

void cast_rays_3d() {
  //Raycaster - Cast rays from camera position and convert them into a "3D" view of the room
  float ray_x, ray_y, ray_angle, x_off, y_off, distance;
  int r_map_x, r_map_y, r_map_pos, ray_iterations;

  //Number of rays - calculate how wide each segment on the screen is based off of this value
  unsigned char rays = 40;
  unsigned char segment_size = floor(float(SCREEN_WIDTH)/float(rays));

  //Set the initial ray angle
  ray_angle = cam_angle - DR*(rays/2);

  //Cast the rays
  for(int r = 0; r < rays; r++){
    //The "ray step" - how many times the ray has moved from its start position
    ray_iterations = 0;

    //Set the ray start position
    ray_x = cam_x;
    ray_y = cam_y;

    //Set the offset by using cos/sin. There are many different ways of achieving this.
    x_off = cos(ray_angle)*1;
    y_off = sin(ray_angle)*1; //Improve resolution of 

    //What wall have we collided with
    int wall_data = 0;

    //Move the ray through the room
    while(ray_iterations < 8*map_offset){ //optimize later
      //Convert ray position in room to map grid
      r_map_x = (ray_x)/map_offset;
      r_map_y = (ray_y)/map_offset;
      r_map_pos = r_map_y*map_x + r_map_x;

      //Check for collision
      if(r_map_pos < map_size && map_grid[r_map_pos] != 0){
        ray_iterations = 10000; //Stop loop
        //Collided with wall of X type
        wall_data = map_grid[r_map_pos];
      }else{
        //Move ray
        ray_x += x_off;
        ray_y += y_off;
        ray_iterations++;
      }
    }
    
   
    //Final distance of ray
    distance = dist(cam_x, cam_y, ray_x, ray_y, ray_angle)*0.75;
    
    //Render ray in 3d space
    float ca = cam_angle-ray_angle;
    if(ca > 2*PI) ca -= 2*PI;
    if(ca < 0) ca += 2*PI;

    //Render ray
    if(distance <= 40){
      float line_h = (SCREEN_HEIGHT)/distance*3;
      float dark = 20;//+(5*(cos(millis()*0.01))); //Flicker light effect
      //Render the wall based off of the calculated distance
      renderWall(r*segment_size, cam_z + (SCREEN_HEIGHT - line_h)/2, segment_size, line_h, distance, dark, wall_data);
    }

    //Increase the ray angle
    ray_angle += DR;
  }
}


void renderWall(int x1, int y1, int w, int h, float dist, float darkness, int wall_data){
  //Render the wall in 3D space
  int vv = (max(1, (min(40, dist)/40)*darkness)); //Tells the dither effect how far away we are from the camera position

  //Render differently depending on which wall type we are
  if(wall_data == 1){
    //Draw dot grid dither effect. This uses the modulo of "vv", which is calculated from the distance value. This means that as the distance increases, the number of displayed pixels will decrease.
    for(int i = 0; i < (w/2); i++){
      for(int j = 0; j < (h/2); j++){
          if((i+j)%vv == 0) display.drawPixel(x1+i*2, y1+j*2, SSD1306_WHITE);
      }
    }
  }else{
    //Exit - so display differently to a regular wall
    display.fillRect(x1, y1, w, h, SSD1306_WHITE);
  }
}

int roundnear (int a) {
  //Rounds a number to its nearest value
  return a >= 0 ? (a+2)/map_offset*map_offset : (a-2)/map_offset*map_offset;
}


float dist(float x1, float y1, float x2, float y2, float ang) {
  //Returns the distance from [x1, y1] to [x2, y2] using pythagoras
  return( sqrt((x2-x1)*(x2-x1) + (y2-y1)*(y2-y1)) );
}

float lerp(float x1, float x2, float p){
  //Interpolates from X1 to X2 at speed P
  return(x1 + p * (x2 - x1));
}

void display_time(int mins, int secs, float progress) {
  //Displays the clock time on the screen
  
  //Calculate the hours and minutes to be displayed based on input info
  int display_hours = mins / 60;
  int display_minutes = mins % 60;
  
  //12 hour clock
  if(display_hours > 12) display_hours-=12;

  //Write clock to display
  display.write("The time is:");
  
  //Make the font bigger and in the centre of the screen
  display.setTextSize(2);      
  display.setCursor((5*10)/2 - (2*5)/2, 24);
  
  //Print out the time
  display.print(display_hours / 10 % 10);
  display.print(display_hours % 10);
  
  //Flash the point to show user that the clock is ticking
  if(seconds_progress <= 0.5){
    display.print(":");
  }else{
    display.print(" ");
  }
  
  //Finally, render the minutes
  display.print(display_minutes / 10 % 10);
  display.print(display_minutes % 10);
  
  //Reset the font size to display both am/pm and the message at the bottom
  display.setTextSize(1);      // Normal 1:1 pixel scale     // Normal 1:1 pixel scale
  
  //Swap betwen am and pm
  if(mins >= 60*12){
    display.write(" p.m.");
  }else{
    display.write(" a.m.");
  }
  
  //Display footer message
  display.setCursor(8, 64-8-5); 
  display.write("Oh blessed day! :)");
  display.setCursor(0, 0); 
}