//Centurion_Ball for HackerBox 0100
//  see details at HackerBoxes.com
//
//Moves a ball around the screen by tilting board
//   left and right buttons change ball color
//
//Required Libraries:
//  GFX Library for Arduino (by Moon On Our Nation)
//    available by searching Manage Libraries
//  MPU9250_WE by Wolfgang Ewald
//    available by searching Manage Libraries

#include <Arduino_GFX_Library.h>
#include <MPU9250_WE.h>
#include <Wire.h>

#define MPU9250_ADDR 0x68
#define MPU_SDA  0
#define MPU_SCL  1

MPU9250_WE myMPU9250 = MPU9250_WE(MPU9250_ADDR);

#define TFTDIN   11
#define TFTCLK   10
#define TFTDC    8
#define TFTCS    9
#define TFTRST   12

// connect GC9A01 circular display
Arduino_DataBus *bus = new Arduino_SWSPI(TFTDC, TFTCS, TFTCLK, TFTDIN, -1);
Arduino_GFX *gfx = new Arduino_GC9A01(bus, TFTRST, 0, true);

// pins for six switches
#define SW_UP   16
#define SW_DN   18
#define SW_LF   19
#define SW_RT   17
#define SW_A    21
#define SW_B    20

int ball_x=0;
int ball_y=0;
int ball_color=YELLOW;

void setup(void)
{
  Serial.begin(9600);
  Wire.setSDA(MPU_SDA);
  Wire.setSCL(MPU_SCL);
  Wire.begin();
  if(!myMPU9250.init()){
    Serial.println("MPU9250 does not respond");
  }
  else{
    Serial.println("MPU9250 is connected");
  }

  Serial.println("Position you MPU9250 flat and don't move it - calibrating...");
  delay(1000);
  myMPU9250.autoOffsets();
  Serial.println("Done!");
  myMPU9250.setAccRange(MPU9250_ACC_RANGE_2G);
  myMPU9250.enableAccDLPF(true);
  myMPU9250.setAccDLPF(MPU9250_DLPF_6); 

  //button pins needs pulldowns since they close directly to 3V3 (active high)
  pinMode(SW_LF, INPUT_PULLDOWN);
  pinMode(SW_RT, INPUT_PULLDOWN);

  gfx->begin();
  gfx->fillScreen(BLACK);
}

void loop()
{
  SetBallColor();  
  MPUball();
  delay(50);
}

void SetBallColor(){
  if (digitalRead(SW_RT))
    ball_color = RED;
  if (digitalRead(SW_LF))
    ball_color = YELLOW;
}

void MPUball(){
  //erase previous ball
  gfx->fillCircle(ball_x+120, ball_y+120, 10, BLACK);

  //read angles and update ball position
  xyzFloat angle = myMPU9250.getAngles();

  if(angle.x > 0)
    ball_y -= 5;
  if(angle.x < 0)
    ball_y += 5;
  if(angle.y > 0)
    ball_x -= 5;
  if(angle.y < 0)
    ball_x += 5;

  //if ball hits wall, return to center
  int r_sq = ball_x*ball_x + ball_y*ball_y;
  if (r_sq >= 120*120){
    ball_x=0;
    ball_y=0;
  }

  gfx->fillCircle(ball_x+120, ball_y+120, 10, ball_color);
}