/**************************************************************************
Based on Adafruit pong 
 **************************************************************************/

//Screen Size Parameters
int x_pixels = 128;
int y_pixels = 64;

//Paddle Parameters
int paddle_height = 10;
int paddle_width = 3;

int speakerPin = 8;

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

#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 SCREEN_ADDRESS 0x3D

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

// Ball velocities:
SyncVar<int> x_vel = 1;
SyncVar<int> y_vel = 1;

// Ball position:
SyncVar<int> x_pos = 5;
SyncVar<int> y_pos = 32;

// Paddle positions:
SyncVar<int> l_pos = 0;
SyncVar<int> r_pos = 0;

// Player scores
int l_score = 0;
int r_score = 0;

struct PlayerContext
{
  SyncVar<int>* pos;
  int side;
  PlayerContext(SyncVar<int>* p, int s) : pos(p), side(s) {} 
  PlayerContext() {}
};

void player(PlayerContext ctx) {
  while(1) {
    if (x_vel == ctx.side) {
      auto pos = *ctx.pos + y_vel;
      auto r = (int)random(paddle_height / 2);
      if (r % 2 == 0) pos + r; else pos - r;
      if (pos < 0) pos = 0;
      if (pos + paddle_height > y_pixels) pos = y_pixels - paddle_height;
      *ctx.pos = pos;
      delay(150);
    }
  }
}

/////////////////////////////////
/// GAME STUFF
/////////////////////////////////
#define LOGO16_GLCD_HEIGHT 16 
#define LOGO16_GLCD_WIDTH  16 
static const unsigned char PROGMEM logo16_glcd_bmp[] =
{ B00000000, B11000000,
  B00000001, 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 };

void setup()   {
  randomSeed(analogRead(0));

  // by default, we'll generate the high voltage from the 3.3v line internally! (neat!)
  display.begin(SSD1306_SWITCHCAPVCC, SCREEN_ADDRESS);  // initialize with the I2C addr 0x3C (for the 128x32)
  // init done

  setupTasks();
  noInterrupts();
  runTask(player, PlayerContext(&l_pos, -1), 56);
  runTask(player, PlayerContext(&r_pos, +1), 56);
  interrupts();
}

void loop() {
  // Check for ball hitting a wall:
  
  if (x_pos > x_pixels-1){
    ball_reset(false);
    l_score += 1;
    tone(speakerPin, 50, 100);
    delay(50);
    tone(speakerPin, 50, 100);
    }  
  
  else if(x_pos < 0){
    ball_reset(true);
    r_score +=1;
    tone(speakerPin, 50, 100);
    delay(50);
    tone(speakerPin, 50, 100);
    }

  // Check for ball bouncing off ceiling:
    
  if (y_pos > y_pixels-1 || y_pos < 0){y_vel = -y_vel;}

  // Check for ball bouncing off paddle:

  // Update ball position:
  x_pos+=x_vel;
  y_pos+=y_vel;

  // Draw pong elements to display:
  display.clearDisplay();
  display.drawPixel(x_pos, y_pos, WHITE);
  display.fillRect(0, l_pos, paddle_width, paddle_height, WHITE);
  display.fillRect(x_pixels-paddle_width ,r_pos, paddle_width, paddle_height, WHITE);

  // Display scores
  display.setTextSize(1);
  display.setTextColor(WHITE);
  display.setCursor(display.width()/4,0);
  display.println(l_score);
  display.setCursor(display.width()*3/4,0);
  display.println(r_score);

  // Display all elements
  display.display();

   // Check for ball bouncing off paddles:
  if (ball_on_right_paddle()){
    x_vel = -x_vel;
    tone(speakerPin, 300, 100);
  }
  else if (ball_on_left_paddle()){
    x_vel = -x_vel;
    tone(speakerPin, 250, 100);
  }
}

bool ball_on_right_paddle(){
// If ball is heading towards paddle and is at the surface of paddle between the top and bottom of the paddle, then it's a hit
return(x_pos == x_pixels-paddle_width-1 && y_pos >= r_pos && y_pos <= (r_pos + paddle_height) && x_vel == 1);
}

bool ball_on_left_paddle(){
return(x_pos == paddle_width-1 && y_pos >= l_pos && y_pos <= (l_pos + paddle_height) && x_vel == -1);
}

void ball_reset(bool left){
//If left is true, then ball should launch from the left, otherwise launch from the right
//Ball should launch at a random Y location and at a random Y velocity

  y_pos = random(display.height());
  if (random(2)>0){
    y_vel = 1;
  }
  else{
    y_vel = -1;
  }

  if (left){
  x_vel = 1;
  x_pos = paddle_width-1;
  }
  
  else{
  x_vel = -1;
  x_pos = display.width()-paddle_width;  
  }
}