/**************************************************************************
 This is an example for our Monochrome OLEDs based on SSD1306 drivers

 Pick one up today in the adafruit shop!
 ------> http://www.adafruit.com/category/63_98

 This example is for a 128x64 pixel display using I2C to communicate
 3 pins are required to interface (two I2C and one reset).

 Adafruit invests time and resources providing this open
 source code, please support Adafruit and open-source
 hardware by purchasing products from Adafruit!

 Written by Limor Fried/Ladyada for Adafruit Industries,
 with contributions from the open source community.
 BSD license, check license.txt for more information
 All text above, and the splash screen below must be
 included in any redistribution.
 **************************************************************************/

#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)
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 
};

int 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, 
};

//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;

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

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;

//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,1,
  1,1,1,1,1,0,0,1,
  1,0,0,1,1,0,0,1,
  1,0,0,1,0,0,0,1,
  1,0,0,1,0,0,0,1,
  1,1,1,1,1,1,1,1
};

//GAMEPLAY FUNCTIONALITY
void setup() {
  Serial.begin(9600);

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

  //Camera Position
  cam_x = 10;
  cam_y = 10;
  cam_z = 0;
  cam_angle = 0;
  cam_delta_x = cos(cam_angle);
  cam_delta_y = sin(cam_angle);


  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, 0x3D)) { // 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
  }

  // Show initial display buffer contents on the screen --
  // the library initializes this with an Adafruit splash screen.
  display.display();
  delay(100); // Pause for 2 seconds
}

void loop() {
  //Refresh rate - Update Display
  display.display();
  display.clearDisplay();
  delay(10);

  //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);

  //Check Inputs and apply movement
  if(left_state == HIGH) { //Move Left
    cam_angle -= 0.1;
    if(cam_angle < 0) cam_angle += 2*PI;
    hand_x_offset += 2;
  }
  
  if(right_state == HIGH) { //Move Right
    cam_angle += 0.1;
    if(cam_angle > 2*PI) cam_angle -= 2*PI;
    hand_x_offset -= 2;
  }

  //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;

  cam_x += move_x;
  cam_y += move_y;

  //Display Map
  map_display = false;
  if(switch_state == HIGH) { //Open Map
    map_display = true;
  }

  //Render
  render_display();
}

//Render Function
void render_display() {
  if(map_display){
    draw_map_2d();
    draw_player();
  }else{
    //3d view
    cast_rays_3d();

    //fps hand
    draw_ui();
  }
}

//FUNCTION STUFF
void draw_map_2d() {
  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() {
  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;

    move_step += 0.6*yspd;
  }else{
    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 Health and Armour
  display.setCursor(2, SCREEN_HEIGHT-8);
  display.write(43);
  display.print(String(player_health));

  //display.setCursor(28, SCREEN_HEIGHT-8);

  //display.write(31);
  //display.print(String(100));

  //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);
}

void cast_rays_3d() {
  float ray_x, ray_y, ray_angle, x_off, y_off, distance;
  int r_map_x, r_map_y, r_map_pos, ray_iterations;

  int rays = 20;
  float segment_size = float(SCREEN_WIDTH)/float(rays);

  //Set
  ray_angle = cam_angle - DR*(rays/2);
  if(ray_angle > 2*PI) ray_angle -= 2*PI;
  if(ray_angle < 0) ray_angle += 2*PI;

  //Cast
  for(int r = 0; r < rays; r++){
    ray_iterations = 0;

    ray_x = cam_x;
    ray_y = cam_y;
      
    x_off = cos(ray_angle)*1;
    y_off = sin(ray_angle)*1; //Improve resolution of 

    while(ray_iterations < 8*map_offset){ //optimize later
      r_map_x = round(ray_x)/map_offset;
      r_map_y = round(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] == 1){
        ray_iterations = 10000; //Stop loop
      }else{
        ray_x += x_off;
        ray_y += y_off;
        ray_iterations++;
      }
    }
    
    //display.drawLine(cam_x, cam_y, ray_x, ray_y, SSD1306_WHITE);
    distance = dist(cam_x, cam_y, ray_x, ray_y, ray_angle);
    
    //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
    distance *= cos(ca); //Fisheye fix
    if(distance <= 40){
      float line_h = min(64, (SCREEN_HEIGHT)/distance*3);
      //int wallseg = min(max(1, distance/40)*3, 3);
      //Serial.println(wallseg);
      //for(int b = 0; b < wallseg; b++){
        //display.fillRect(r*segment_size + b, (SCREEN_HEIGHT - lh)/2 + b, segment_size-b*2, lh-b*2, SSD1306_INVERSE);
      //}

      float dark = 20;//+(5*(cos(millis()*0.01))); //Flicker light effect

      renderWall(r*segment_size, cam_z + (SCREEN_HEIGHT - line_h)/2, segment_size, line_h, distance, dark);
    }

    ray_angle += DR; 
    if(ray_angle > 2*PI) ray_angle -= 2*PI;
    if(ray_angle < 0) ray_angle += 2*PI;
  }

  /*int mx, my, mp, dof; float ray_x, ray_y, ray_angle, x_off, y_off;
  ray_angle = cam_angle;

  //Cast rays
  for(int r = 0; r < 1; r++){
    //Horizontal
    dof = 0;
    float arctan = -1/tan(ray_angle);

    //Check ray angle
    if(ray_angle > PI) {
      ray_y = roundnear(cam_y)-float(map_offset);
      ray_x = (cam_y-ray_y)*arctan + cam_x;

      y_off = -map_offset;
      x_off = -y_off*arctan;
    }
    if(ray_angle < PI) {
      ray_y = roundnear(cam_y)+float(map_offset);
      ray_x = (cam_y-ray_y)*arctan + cam_x;

      y_off = -map_offset;
      x_off = -y_off*arctan;
    }
    if(ray_angle == 0 || ray_angle == PI) {
      ray_x = cam_x;
      ray_y = cam_y;
      dof = 8;
    }

    while(dof < 8){
      mx = roundnear(cam_x)/float(map_offset);
      my = roundnear(cam_y)/float(map_offset);
      mp = my*map_x+mx;

      //Collided with wall
      if(mp < map_x*map_y && map[mp] == 1) {
        dof = 8;
      }else{
        ray_x += x_off;
        ray_y += y_off;
        dof += 1;
      }
    }
    display.drawLine(cam_x, cam_y, ray_x, ray_y, SSD1306_WHITE);
  }*/
}


void renderWall(int x1, int y1, int w, int h, float dist, float darkness){
  int vv = floor(max(1, (min(40, dist)/40)*darkness));
  
  for(int i = 0; i < (w/2); i++){
    for(int j = 0; j < (h/2); j++){
        if((i+j)%vv == 0) display.fillRect(x1+i*2, y1+j*2, 2, 2, SSD1306_WHITE);
    }
  }
}

int roundnear (int a)
{
  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) {
  return( sqrt((x2-x1)*(x2-x1) + (y2-y1)*(y2-y1)) );
}

float lerp(float x1, float x2, float p){
  return(x1 + p * (x2 - x1));
}


/* Redundant
void loop()
{
  //Refresh rate
  delay(5);
  long int render_time_initial = millis();
  display.display();
  display.clearDisplay();

  long int t = millis();

  int sw = SCREEN_WIDTH/2;
  int sh = SCREEN_HEIGHT/2;

  for(int i = -sw; i < sw; i++){
    float line_h = abs(sin(i*0.1 + t*0.0025))*SCREEN_HEIGHT;
    display.drawFastVLine(sw + i, (SCREEN_HEIGHT - line_h)/2, line_h, SSD1306_WHITE);
    //display.drawPixel(sw + i, sh + line_h, SSD1306_WHITE);
  }


  long int render_time_final = millis();

  Serial.println(String(render_time_final-render_time_initial) + "ms render time"); 
  //avg with 1 line ~38ms
  //if line h is recalculated, upwards of 50 ms per frame
}

void testdrawbitmap() {
  display.clearDisplay();

  display.drawBitmap(0, 0, logo_bmp, LOGO_WIDTH, LOGO_HEIGHT, 1);
  display.drawBitmap(5, 5, logo_bmp, LOGO_WIDTH, LOGO_HEIGHT, 0);
  display.drawBitmap(10, 10, logo_bmp, LOGO_WIDTH, LOGO_HEIGHT, 1);
  display.display();
  delay(1000);
}

void teststr() {
    display.clearDisplay();

    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

    // Not all the characters will fit on the display. This is normal.
    // Library will draw what it can and the rest will be clipped.
    display.write("Hello, world!");
    display.setCursor(0, FONT_HEIGHT);     // Start at top-left corner
    for(int i = 0; i < 256; i++){   
        if(i == '\n') display.write(' ');
        else          display.write(i);
    }
    display.display();
}
*/