//Jacob Lehrman
//Me 608, Dr. Wilson
//University of Kansas
//Last Updated: 5/15/25
//Desc: Code to test screen output. Screen is 2.42" 128x64 monochrome OLED from Adafruit driven by SSD1309 IC.
//The code has metastasized :(

//External Libraries
#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_BNO055.h>

//Objects

Adafruit_BNO055 bno = Adafruit_BNO055(55, 0x28);

//Preallocated Variables

const static uint16_t spriteSheet[44] = {0x4C44,0x44E0,0xE22E,0x88E0,0x22E0,0xAAAE,0x2220,0xE88E,0xAAE0,0xE222,0xEAAE,0xEAAA,0x8880,0xAAAA,0xEA8E,0x2AE0,0xAAA0,0xE444,0x4440,0xCAAC,0x8888,0x8080,0x44E0,0xE888,0xAA40,0x0525,0x0345,0x3488,0x37EC,0xB4A8,0x8421,0xFFFF,0xA5A5,0xF510,0xF551,0x348C,0xB430,0x0348,0xC730,0xCFD1,0xCFF3,0xCFB0,0xB5B5,0x8800};//All sprites in order. Written number is off-by-one from indices.
const uint8_t numSprites = 40;//Number of sprites the program can hold
const uint8_t numLines = 64;//Number of renderlines
const uint8_t overflow = 7;//Number of additional lines in the buffer to make everything line up correctly
const uint8_t numMines = 8;//Number of Landmines allowed on screen at once
uint64_t buffer[numLines+overflow];//Allocate ram for renderlines
uint16_t sprites[numSprites];//First 16 are score, next 4 are Car, next 6 are shift bar, next 12 are SHIFT! message. These are the always-present entities of the game
uint8_t spritePos[2][numSprites];//Sprite Positions. First array is x, second is y. Note that +x and +y directions are fully inverted from convention. In display terms, +x goes left from the right side, +y goes down from the top side.
uint16_t background[5];//Ram for the background sprites.
uint16_t landmines[4];//Ram for the landmines
uint8_t landminePos[2][numMines];//Landmine positions. Same convention as sprites
uint8_t minesShown = 0;//DO NOT CHANGE THIS! This stores 8 boolean values to determine which mines should be rendered, if any.
bool alive = false;//Whether the car has died or not. Initializes to false until the calibration step is done
bool stop = false;//Whether the game should be playing or not
bool starting = true;//If the game is currently in the calibration/menu phase
bool buttonPhase = false;//Which frame the start menu button should be showing
uint32_t score = 0;
uint8_t frameCount = 0;//Holds the number of frames
int startTime;
uint8_t bufferQuat[8];//For buffering quaternion components from BNO055
int16_t quatComp[4];//Quaternion Components. Order w, x, y, z
const uint8_t numThresholds = 8;//Number of demarcation lines for scaling BNO055 output
int16_t thresholds[2][numThresholds];//Array of the values for scaling Quaternion input to more reasonable values. First is w, last is z.
uint8_t currentRegion;//Value to hold which region the quaternion component(s) fall into
uint8_t scrollSpeed = 1;//Controls how fast the background and mines move, and how fast score is gained.

//Function Block

void setText(uint8_t charVal, uint8_t xPos, uint8_t yPos, uint8_t spriteIndex){//Sprite Index must be externally incremented by 2
  //Takes a number and turns it into sprites at the input positions
  switch(charVal){
    case 0:
      setSprite(xPos,yPos,spriteIndex,11);
      spriteIndex++;
      setSprite(xPos,yPos+4,spriteIndex,8);
      break;
    case 1:
      setSprite(xPos,yPos,spriteIndex,0);
      spriteIndex++;
      setSprite(xPos,yPos+4,spriteIndex,1);
      break;
    case 2:
      setSprite(xPos,yPos,spriteIndex,2);
      spriteIndex++;
      setSprite(xPos,yPos+4,spriteIndex,3);
      break;
    case 3:
      setSprite(xPos,yPos,spriteIndex,2);
      spriteIndex++;
      setSprite(xPos,yPos+4,spriteIndex,4);
      break;
    case 4:
      setSprite(xPos,yPos,spriteIndex,5);
      spriteIndex++;
      setSprite(xPos,yPos+4,spriteIndex,6);
      break;
    case 5:
      setSprite(xPos,yPos,spriteIndex,7);
      spriteIndex++;
      setSprite(xPos,yPos+4,spriteIndex,4);
      break;
    case 6:
      setSprite(xPos,yPos,spriteIndex,7);
      spriteIndex++;
      setSprite(xPos,yPos+4,spriteIndex,8);
      break;
    case 7:
      setSprite(xPos,yPos,spriteIndex,9);
      spriteIndex++;
      setSprite(xPos,yPos+4,spriteIndex,6);
      break;
    case 8:
      setSprite(xPos,yPos,spriteIndex,10);
      spriteIndex++;
      setSprite(xPos,yPos+4,spriteIndex,8);
      break;
    case 9:
      setSprite(xPos,yPos,spriteIndex,10);
      spriteIndex++;
      setSprite(xPos,yPos+4,spriteIndex,6);
      break;
    case 10:
      setSprite(xPos,yPos,spriteIndex,10);
      spriteIndex++;
      setSprite(xPos,yPos+4,spriteIndex,16);
      break;
    case 11:
      setSprite(xPos,yPos,spriteIndex,23);
      spriteIndex++;
      setSprite(xPos,yPos+4,spriteIndex,3);
      break;
    case 12:
      setSprite(xPos,yPos,spriteIndex,7);
      spriteIndex++;
      setSprite(xPos,yPos+4,spriteIndex,3);
      break;
    case 13:
      setSprite(xPos,yPos,spriteIndex,7);
      spriteIndex++;
      setSprite(xPos,yPos+4,spriteIndex,12);
      break;
    case 14:
      setSprite(xPos,yPos,spriteIndex,5);
      spriteIndex++;
      setSprite(xPos,yPos+4,spriteIndex,16);
      break;
    case 15:
      setSprite(xPos,yPos,spriteIndex,17);
      spriteIndex++;
      setSprite(xPos,yPos+4,spriteIndex,22);
      break;
    case 16:
      setSprite(xPos,yPos,spriteIndex,10);
      spriteIndex++;
      setSprite(xPos,yPos+4,spriteIndex,12);
      break;
    case 17:
      setSprite(xPos,yPos,spriteIndex,19);
      spriteIndex++;
      setSprite(xPos,yPos+4,spriteIndex,16);
      break;
    case 18:
      setSprite(xPos,yPos,spriteIndex,14);
      spriteIndex++;
      setSprite(xPos,yPos+4,spriteIndex,15);
      break;
    case 19:
      setSprite(xPos,yPos,spriteIndex,17);
      spriteIndex++;
      setSprite(xPos,yPos+4,spriteIndex,18);
      break;
    case 20:
      setSprite(xPos,yPos,spriteIndex,13);
      spriteIndex++;
      setSprite(xPos,yPos+4,spriteIndex,8);
      break;
    case 21:
      setSprite(xPos,yPos,spriteIndex,13);
      spriteIndex++;
      setSprite(xPos,yPos+4,spriteIndex,24);
      break;
    case 22:
      setSprite(xPos,yPos,spriteIndex,20);
      spriteIndex++;
      setSprite(xPos,yPos+4,spriteIndex,21);
      break;
    default:
      return;
  }
}

void setSprite(uint8_t x, uint8_t y, uint8_t index, uint8_t ref){
  //Puts a sprite into memory alsong with its position
  sprites[index] = spriteSheet[ref];//Fetches the sprite
  spritePos[0][index] = x;//Sets X coordinate
  spritePos[1][index] = y;//Sets Y coordinate
}

void renderFrameTop(){
  //Renders the top half of the screen
  //First loop to render the always-present sprites
  for(uint8_t count=0;count<numSprites;count++){
    uint8_t y = spritePos[1][count];//Makes this easier to type
    uint8_t x = spritePos[0][count];
    if(y < 64){
      uint8_t difference = 64-y;//How far away the sprite is from the endline
      if(difference > 4){
        difference = 4;//since sprites only occupy 4 lines, clamp value to that if greater
      }
      for(uint8_t i=0;i<difference;i++){
        buffer[y+i] |= (getLine(sprites[count],i) << (x-4));//Writes sprite Line to buffer Line
      }
    }
  }
  if(!starting){
    renderMinesTop();
    renderBackground(8);
  }
  bufferOut();//Output the Top of the rendered Frame
}

void renderFrameBottom(){
  //Renders bottom half of the screen.
  //First loop to render the always-present sprites
  for(uint8_t count=0;count<numSprites;count++){
    uint8_t y = spritePos[1][count];//Makes this easier to type
    uint8_t x = spritePos[0][count];
    if(y > 63){//No need to check for the endline; can remove erroneous sprites before rendering
      y-=64;//Brings y-value into Buffer range.
      uint8_t difference = 64-y;//How far away the sprite is from the endline
      if(difference > 4){
        difference = 4;//since sprites only occupy 4 lines, clamp value to that if greater
      }
      for(uint8_t i=0;i<difference;i++){
        buffer[y+i] |= (getLine(sprites[count],i) << (x-4));//Writes sprite Line to buffer Line
      }
    }
  }
  if(!starting){
    renderMinesBottom();
    renderBackground(0);
  }
  bufferOut();//Output the bottom of the rendered Frame
}

void bufferOut(){
  //Fuction to output the entire Buffer to the display
  for(uint8_t j=0;j<numLines;j++){
    for(uint8_t i=0;i<8;i++){
      uint8_t byte = buffer[j];//Saves the byte corresponding to the current position in the display line
      buffer[j] >>= 8;//Removes the previous byte.
      writeByte(byte);//Outputs the byte to the display
    }
  }
  return;
}

void renderBackground(uint8_t startPos){
  //Renders the background sprites to the buffer
  for(uint8_t i=startPos;i<64;i+=4){
    for(uint8_t j=0;j<4;j++){
      buffer[i+j] |= getLine(background[0],j) << 60;
      buffer[i+j] |= getLine(background[1],j) << 56;
      buffer[i+j] |= getLine(background[2],j) << 28;
      buffer[i+j] |= getLine(background[3],j) << 4;
      buffer[i+j] |= getLine(background[4],j);
    }
  }
}

void renderMinesTop(){
  //Renders active landmines to the screen
  uint8_t compare = 0b10000000;//Worker value for boolean conversion
  for(uint8_t count=0;count<numMines;count++){
    if(minesShown & compare){
      uint8_t x = landminePos[0][count];//mine X and y coordinates
      uint8_t y = landminePos[1][count];//Doing this for convenience

      if(y < numLines){//check to see if the mine is in range of the buffer
        for(uint8_t i=0;i<4;i++){//Can do all on one line thanks to the buffer overflow
          buffer[y+i] |= (getLine(landmines[0],i) << (x-4));//Writes first sprite Line to buffer
          buffer[y+i] |= (getLine(landmines[1],i) << (x-8));//Writes second sprite Line to buffer
          buffer[y+i+4] |= (getLine(landmines[2],i) << (x-4));//Writes third sprite Line to buffer
          buffer[y+i+4] |= (getLine(landmines[3],i) << (x-8));//Writes fourth sprite Line to buffer
        }
      }
    }
    compare >>= 1;//Shifts comapre by 1 to check the next mine
  }
}

void renderMinesBottom(){
  //Renders active landmines to the bottom of the screen
  uint8_t compare = 0b10000000;//Worker value for boolean conversion
  for(uint8_t i=0;i<overflow;i++){
    buffer[0+i] = buffer[64+i];//Write overflow lines from previous half to top of this half.
    buffer[64+i] = 0;//Clear overflow lines
  }

  for(uint8_t count=0;count<numMines;count++){
    if(minesShown & compare){
      uint8_t x = landminePos[0][count];//mine X and y coordinates
      uint8_t y = landminePos[1][count];//Doing this for convenience

      y-=numLines;//reduces y values to be rendered here into buffer range, and underflows lesser values!

      if(y < numLines){//check to see if the first/second sprites are in range of the buffer
        uint8_t difference = numLines-y;//How far away the sprite is from the endline
        if(difference > 4){
          difference = 4;//since sprites only occupy 4 lines, clamp value to that if greater
        }
        for(uint8_t i=0;i<difference;i++){
          buffer[y+i] |= (getLine(landmines[0],i) << (x-4));//Writes first sprite Line to buffer
          buffer[y+i] |= (getLine(landmines[1],i) << (x-8));//Writes second sprite Line to buffer
        }
      }

      if(y < (numLines-4)){//Check if the third/fourth sprites are within the current buffer
        uint8_t difference = numLines-(y+4);//How far away the sprite is from the endline
        if(difference > 4){
          difference = 4;//since sprites only occupy 4 lines, clamp value to that if greater
        }
        for(uint8_t i=0;i<difference;i++){
          buffer[y+i+4] |= (getLine(landmines[2],i) << (x-4));//Writes third sprite Line to buffer
          buffer[y+i+4] |= (getLine(landmines[3],i) << (x-8));//Writes fourth sprite Line to buffer
        }
      }
    }
    compare >>= 1;//Shifts comapre by 1 to check the next mine
  }
}

void setBackground(){
  //Called during setup to load the background sprites
  background[0] = spriteSheet[32];
  background[1] = spriteSheet[42];
  background[2] = spriteSheet[43];
  background[3] = flipH(spriteSheet[42]);
  background[4] = flipH(spriteSheet[32]);
}

void setMines(){
  //Call during setup to fill landmine sprite array
  landmines[0] = spriteSheet[25];
  landmines[1] = flipH(spriteSheet[25]);
  landmines[2] = flipV(spriteSheet[25]);
  landmines[3] = flipD(spriteSheet[25]);
}

void scrollBackground(uint8_t scroll){
  //Scrolls the background.
  for(uint8_t i=0;i<5;i++){
    background[i] = scrollV(background[i],scroll);
  }
}

uint64_t getLine(uint16_t sprite, uint8_t line){
  //Isolates a single line of a 4x4 sprite to render. Line variable must be externally constrained to range 0:3
  uint64_t lineOut = 0;
  switch(line){
    case 0:
      lineOut = (sprite & 0xF000) >> 12;//Grabs first line
      break;
    case 1:
      lineOut = (sprite & 0x0F00) >> 8;//Grabs second line
      break;
    case 2:
      lineOut = (sprite & 0x00F0) >> 4;//Grabs third line
      break;
    case 3:
      lineOut = sprite & 0x000F;//Grabs fourth line
      break;
  }
  return lineOut;
}

void spawnMine(uint8_t xPos){
  uint8_t compare = 0b10000000;//Worker Value for comparisons
  for(uint8_t i=0;i<numMines;i++){
    if(~minesShown & compare){//Check if there is an available mine slot to spawn a mine into
      landminePos[0][i] = xPos;
      landminePos[1][i] = 8;//Mines always start at top of track
      minesShown |= compare;//Set the mine position in memory as occupied
      return;//Leave function at this point. POSSIBLE MEMORY LEAK
    }
    compare >>= 1;
  }
}

void updateMine(uint8_t scroll){
  //Scrolls mines downwards and removes offscreen mines
  uint8_t compare = 0b10000000;
  for(uint8_t i=0;i<numMines;i++){
    if(minesShown & compare){//Check if there is a mine in the slot
      landminePos[1][i] += scroll;//Adding moves mines downwards
      if(landminePos[1][i] > 127){//Check if mines are offscreen
        minesShown &= ~compare;//Clear mine position
      }
    }
    compare >>= 1;
  }
}

void checkCollision(){
  //Check if the car intersects any of the landmines
  uint8_t compare = 0b10000000;
  for(uint8_t i=0;i<numMines;i++){
    if(minesShown & compare){//Check if there is a mine in the slot
      if(landminePos[1][i] > 111){//If the landmine is in the right Y-level
      int8_t diff = landminePos[0][i]-spritePos[0][16];
        if(diff>-7 && diff<7){
          alive = false;
          frameCount = 0;//Bookeeping
          minesShown &= ~compare;//"""Detonate""" the mine if contacted
        }
      }
    }
    compare >>= 1;
  }
  //check if the car is on the shoulder
  if(spritePos[0][16]>56 || spritePos[0][16]<16){
    alive = false;
    frameCount = 0;//Bookeeping
  }
}

void loadCar(uint8_t mode, uint8_t xPos){
  //Abstraction to make loading/unloading the car easier
  switch(mode){
    case 0:
      sprites[16] = spriteSheet[39];
      sprites[17] = flipH(spriteSheet[39]);
      sprites[18] = flipV(spriteSheet[39]);
      sprites[19] = flipD(spriteSheet[39]);
      break;
    case 1:
      sprites[16] = spriteSheet[40];
      sprites[17] = flipH(spriteSheet[41]);
      sprites[18] = flipV(spriteSheet[40]);
      sprites[19] = flipD(spriteSheet[41]);
      break;
    case 2:
      sprites[16] = spriteSheet[41];
      sprites[17] = flipH(spriteSheet[40]);
      sprites[18] = flipV(spriteSheet[41]);
      sprites[19] = flipD(spriteSheet[40]);
      break;
  }
  uint8_t iter = 0;//For tracking what sprite index to output to
  for(uint8_t y=0;y<5;y+=4){
    for(uint8_t x=0;x<5;x+=4){
      spritePos[0][16+iter] = xPos-x;//Minus so it outputs on the correct side
      spritePos[1][16+iter] = 120+y;
      iter++;
    }
  }
}

void kill(){
  //Plays the death explosion
  uint8_t stage = frameCount/6;//Animation frame update every 6 frames
  switch(stage){
    case 0:
      sprites[16] = spriteSheet[26];
      sprites[17] = flipH(spriteSheet[26]);
      sprites[18] = flipV(spriteSheet[26]);
      sprites[19] = flipD(spriteSheet[26]);
      break;
    case 1:
      sprites[16] = spriteSheet[27];
      sprites[17] = flipH(spriteSheet[27]);
      sprites[18] = flipV(spriteSheet[27]);
      sprites[19] = flipD(spriteSheet[27]);
      break;
    case 2:
      sprites[16] = spriteSheet[28];
      sprites[17] = flipH(spriteSheet[28]);
      sprites[18] = flipV(spriteSheet[28]);
      sprites[19] = flipD(spriteSheet[28]);
      break;
    case 3:
      sprites[16] = spriteSheet[29];
      sprites[17] = flipH(spriteSheet[29]);
      sprites[18] = flipV(spriteSheet[29]);
      sprites[19] = flipD(spriteSheet[29]);
      break;
    default:
      if(~frameCount%2){
        sprites[16] = spriteSheet[30];
        sprites[17] = flipH(spriteSheet[30]);
        sprites[18] = flipV(spriteSheet[30]);
        sprites[19] = flipD(spriteSheet[30]);
      }else{
        sprites[16] = 0;
        sprites[17] = 0;
        sprites[18] = 0;
        sprites[19] = 0;
      }
  }
}

void calibrate(){
  //Called during setup; forces program to wait until sensor is calibrated to start game
  digitalWrite(A1,LOW);// No audio during calibration
  digitalWrite(A2,LOW);
  uint8_t sys, gyro, acc, mag;
  bno.getCalibration(&sys,&gyro,&acc,&mag);//Fetches calibration status
  while(sys!=3){//First entry is system calibration. Can check others if needed.
    setText(sys,64,0,0);//Displays the system calibration as first
    setText(gyro,60,0,2);//Displays the Gyro calibration status
    setText(acc,56,0,4);//Displays the Accel calibration status
    setText(mag,52,0,6);//Displays the Magnetometer calibration status
    //Renders a frame
    renderFrameTop();
    renderFrameBottom();
    delay(16);//Caps framerate to 60 fps; dont need to render more than that
    bno.getCalibration(&sys,&gyro,&acc,&mag);//Fetches calibration status
  }
  //At this point the sensor is presumed calibrated, so now to get calibration values for the range of the sensor in relevant axes
  //Experimentally, w and z should vary in phase with each other for left/right rolling
  //Experimentally, w and z should vary exactly out of phase with each other for up/down motion
  //Currently only care about left/right movement, so scale those two dimensions

  //FT = Left; means to roll as far left as necessary
  setText(13,64,8,8);//Letter F
  setText(19,60,8,10);//Letter T
  while(digitalRead(A0) == HIGH){//Holds program until button is pressed
    //Display the value read out from the sensor while calibrating so that if the sensor disconnects, that can be known.
    getQuat();
    uint16_t dummyScore = quatComp[0];//Isolate w component for manip
    for(uint8_t i=12;i<22;i+=2){//print score
      uint8_t letter = dummyScore%10;//Isolates the rightmost Decimal digit from the value
      setText(letter,16+(i*2),8,i);//Displays the isolated digit
      dummyScore /= 10;//Removes the rightmost digit. since dummyScore is an int, division truncates any decimal portion.
    }
    renderFrameTop();//Must always render the whole frame; otherwise desync
    renderFrameBottom();
    delay(16);//Cap to 60 fps
  }
  getQuat();//Button pressed means sensor is rolled to maximum left, so read Values.
  int16_t leftValues[2] = {quatComp[0],quatComp[3]};//w and z components, saved.
  delay(2000);//To allow the button to release
  //RI = right; Roll as far right as necessary
  setText(17,64,16,22);//Letter R
  setText(15,60,16,24);//Letter I
  while(digitalRead(A0) == HIGH){
    //Display the value read out from the sensor while calibrating so that if the sensor disconnects, that can be known.
    getQuat();
    uint16_t dummyScore = quatComp[0];//Isolate w component for manip
    for(uint8_t i=26;i<36;i+=2){//print score
      uint8_t letter = dummyScore%10;//Isolates the rightmost Decimal digit from the value
      setText(letter,-16+(i*2),16,i);//Displays the isolated digit
      dummyScore /= 10;//Removes the rightmost digit. since dummyScore is an int, division truncates any decimal portion.
    }
    renderFrameTop();
    renderFrameBottom();
    delay(16);
    }//Holds program until button is pressed
  getQuat();//Button pressed means sensor is rolled to maximum right, so read Values.
  //Now scale into ranges
  int16_t difference_w = quatComp[0]-leftValues[0];//Differnece in w values left to right
  int16_t difference_z = quatComp[3]-leftValues[1];//Difference in z values left to right
  int16_t region_w = difference_w/numThresholds;//Size of intervals
  int16_t region_z = difference_z/numThresholds;
  thresholds[0][0] = leftValues[0] + region_w;//First threshold value
  thresholds[1][0] = leftValues[1] + region_z;
  for(uint8_t i=1;i<numThresholds;i++){
    thresholds[0][i] = thresholds[0][i-1] + region_w;//Computes remaining threshold values
    thresholds[1][i] = thresholds[1][i-1] + region_z;
  }
  writeByte(0x00);//Makes the screen line up
  delay(1000);//To allow the button to release
}

void startScreen(){
  digitalWrite(A1, HIGH);//Bass + Arpeggio out
  //Idle screen for between games
  setText(16,46,56,0);//Letter P
  setText(20,42,56,2);//Letter U
  setText(18,38,56,4);//Letter S
  setText(14,34,56,6);//Letter H

  setText(19,50,64,8);//Letter T
  setText(0,46,64,10);//Letter O/0

  setText(18,38,64,12);//Letter S
  setText(19,34,64,14);//Letter T
  setText(10,30,64,16);//Letter A
  setText(17,26,64,18);//Letter R
  setText(19,22,64,20);//Letter T
  setText(22,18,64,22);//Letter !

  if(frameCount%32 == 0){buttonPhase = !buttonPhase;}//Toggle the animation frame every 32 rendered frames
  if(buttonPhase){
    //Renders the button in its down position
    setSpriteImproved(26,56,24,spriteSheet[37]);
    setSpriteImproved(22,56,25,flipH(spriteSheet[37]));
    setSpriteImproved(26,60,26,spriteSheet[38]);
    setSpriteImproved(22,60,27,flipH(spriteSheet[38]));
  }else{
    //Renders the button in its up position
    setSpriteImproved(26,56,24,spriteSheet[35]);
    setSpriteImproved(22,56,25,flipH(spriteSheet[35]));
    setSpriteImproved(26,60,26,spriteSheet[36]);
    setSpriteImproved(22,60,27,flipH(spriteSheet[36]));
  }
  if(digitalRead(A0) == LOW){//Checks for the button to be pressed
    starting = false;
    digitalWrite(A2, HIGH);//Enable Melody
    alive = true;//Don't be stoopid
    for(uint8_t i=0;i<numSprites;i++){//Clear all sprites that the main frame doesn't immediately clear
      setSpriteImproved(0,0,i,0x0000);
    }
    loadCar(0,36);//Set Car to default position in the center.
  }
}

void setSpriteImproved(uint8_t x, uint8_t y, uint8_t index, uint16_t sprite){
  //Sets a sprite by passing in a sprite, not a reference to the sheet.
  sprites[index] = sprite;//Writes the sprite
  spritePos[0][index] = x;//Sets X coordinate
  spritePos[1][index] = y;//Sets Y coordinate
}

void convertQuat(){
  //Reads the quaternion and automatically converts it into the appropriate threshold
  getQuat();//Update Quaternion
  for(uint8_t i=0;i<numThresholds;i++){//Only check the w as we dont need to know if the rotation is down or up, only left or right.
    if(quatComp[0]<thresholds[0][i]){//This means it is less than the lowest threshold (should be most left)
      currentRegion = i;//left-most threshold is lowest value
      return;
    }
  }
  if(quatComp[0]>thresholds[0][7]){currentRegion = 8;}//If it gets to here then the value is in the rightmost region.
}

void moveCar(){
  //Converts currentRegion into car motion
  switch(currentRegion){
    case 0:
      loadCar(1,spritePos[0][16]+1);
      break;
    case 1:
      if(frameCount%2){spritePos[0][16]++;}
      loadCar(1,spritePos[0][16]);
      break;
    case 2:
      if(frameCount%3){spritePos[0][16]++;}
      loadCar(1,spritePos[0][16]);
      break;
    case 3:
      if(frameCount%4){spritePos[0][16]++;}
      loadCar(0,spritePos[0][16]);
      break;
    case 4:
      loadCar(0,spritePos[0][16]);
      break;
    case 5:
      if(frameCount%4){spritePos[0][16]--;}
      loadCar(0,spritePos[0][16]);
      break;
    case 6:
      if(frameCount%3){spritePos[0][16]--;}
      loadCar(2,spritePos[0][16]);
      break;
    case 7:
      if(frameCount%2){spritePos[0][16]--;}
      loadCar(2,spritePos[0][16]);
      break;
    case 8:
      loadCar(2,spritePos[0][16]-1);
      break;
  }
}

//Sprite Manipulation Block
uint16_t flipV(uint16_t sprite){
  //Flip a 4x4 sprite Vertically
  sprite = (sprite & 0xFF00) >> 8 | (sprite & 0x00FF) << 8;//Takes the Top two lines and swaps them with the Bottom two
  sprite = (sprite & 0xF0F0) >> 4 | (sprite & 0x0F0F) << 4;//Takes the first and third lines and swaps them with the second and fourth, respectively. 
  return sprite;//This yields a complete vertical flip
}

uint16_t flipH(uint16_t sprite){
  //Flip a 4x4 sprite Horizontally
  sprite = (sprite & 0xCCCC) >> 2 | (sprite & 0x3333) << 2;//Takes the Top two lines and swaps them with the Bottom two
  sprite = (sprite & 0xAAAA) >> 1 | (sprite & 0x5555) << 1;//Takes the first and third lines and swaps them with the second and fourth, respectively. 
  return sprite;//This yields a complete horizontal flip
}

uint16_t flipD(uint16_t sprite){
  //Flip a 4x4 sprite across the 45 degree diagonal
  sprite = flipV(sprite);//Flip Vertically
  sprite = flipH(sprite);//Flip Horizontally
  return sprite;//Complete Diagonal Flip
}

uint16_t scrollV(uint16_t sprite,uint8_t scroll){
  //Scrolls a sprite's horizontal lines vertically downward. Scroll must be externally clamped in the range 0:3
  switch(scroll){
    case 1:
      sprite = (sprite & 0xFFF0) >> 4 | (sprite & 0x000F) << 12;//Slides bottom line to top, other lines down.
      break;
    case 2:
      sprite = (sprite & 0xFF00) >> 8 | (sprite & 0x00FF) << 8;//Swaps bottom and top two lines
      break;
    case 3:
      sprite = (sprite & 0xF000) >> 12 | (sprite & 0x0FFF) << 4;//Slides top line to bottom, other lines up.
      break;
    default:
      ;
  }
  return sprite;
}

//I-O Block
void sendCommand(uint8_t command){
  //Abbreviation so I dont have to write so many of these
  PORTD = command << 2;//Writes the lower 6 bits of the command to data block
  delayMicroseconds(1);
  PORTB = 0b00001000 | (command >> 6);//Sets Write to low to enable byte transmission; Also Writes upper two bits of the command
  delayMicroseconds(1);
  PORTB = PORTB | 0b00011000;//Sets Write to high to trigger leading edge input on SSD1309 display driver
  delayMicroseconds(1);
}

void writeByte(uint8_t data){
  //Simple function to abstract the process of outputting data to the display.
  PORTD = data << 2;//Writes the lower 6 bits of the data to data block
  PORTB = 0b00101000 | (data >> 6);//Sets Write to low to enable byte transmission; Also Writes upper two bits of the data
  PORTB = PORTB | 0b00111000;//Sets Write to high to trigger leading edge input on SSD1309 display driver
}

void getQuat(){
  //Reads out quaternion components into memory
  memset(bufferQuat, 0, 8);//Clear the Buffer
  bno.readLen(Adafruit_BNO055::BNO055_QUATERNION_DATA_W_LSB_ADDR, bufferQuat, 8);//Reads data into buffer. REQUIRES THAT THE LIBRARY BE MODIFIED SO THIS METHOD IS NOT PRIVATE

  for(uint8_t i=0;i<4;i++){
    quatComp[i] = (((uint16_t)bufferQuat[1+(2*i)]) << 8) | ((uint16_t)bufferQuat[(2*i)]);//Converts buffer into actual values
  }
}

void frame(){
  //Master function to orchestrate an entire rendered frame
  if(!starting){
    //Update positions
    if(alive){
      convertQuat();
      moveCar();
      updateMine(scrollSpeed);
      scrollBackground(scrollSpeed);
      if(frameCount%(64 >> scrollSpeed) == 0){
        spawnMine(uint8_t (random(16,56)));
      }
      //Update counters
      score+=scrollSpeed;//Increment score
      if(score>1000){//The longer the player has been racing, the faster (harder) it should get
        scrollSpeed = 2;
        if(score>2500){
          scrollSpeed = 3;
          if(score>5000){
            scrollSpeed = 4;
            if(score>10000){
              scrollSpeed = 5;
            }
          }
        }
      }
      checkCollision();
      int dummyScore = score;//Isolate score for manipulation
      for(uint8_t i=0;i<16;i+=2){//print score
        uint8_t letter = dummyScore%10;//Isolates the rightmost Decimal digit from the value
        setText(letter,4+(i*2),0,i);//Displays the isolated digit
        dummyScore /= 10;//Removes the rightmost digit. since dummyScore is an int, division truncates any decimal portion.
      }
    }else{
      if(frameCount < 120){
        kill();
      }else{
        starting = true;
        score = 0;//Reset Game state
        minesShown = 0;//Removes all mines and disables their collision
        scrollSpeed = 1;//Default scroll speed
        digitalWrite(A2, LOW);//Disable Melody
      }
    }
  }else{
    startScreen();
  }
  //Render Block
  renderFrameTop();
  renderFrameBottom();

  //Timing Block
  frameCount++;//Increment framecounter
  uint16_t diff = micros() - startTime;
  if(diff<16667){
    delayMicroseconds(16667-diff);//To make sure there are 60FPS
  }
  startTime = micros();

}

//Arduino Block
void setup() {
  if(!bno.begin()){while(1);}//Stops program if sensor fails to initialize
  //Initialize Pins
  DDRB = 0x3F;//Enables OUTPUT on pins 13 through 8. 13 is Data/Command, 12 is Write, 11 is reset, 10 is Audio (unused), 9 is DATA7, 8 is DATA6
  DDRD = 0xFC;//Enanles OUTPUT on pins 7:2, DATA5:0
  //Initialize Display
  PORTB = 0b00000000;//Set Reset Pin Low for powerup sequence
  delay(1);
  PORTB = 0b00001000;//Set Reset Pin High for normal operation
  delay(1);//Needed to give Display block time to come online.
  //Command roster provided by manufacturer
  sendCommand(0xFD);//Tells the display to expect command lock instruction
  sendCommand(0x12);//Unlocks the driver
  sendCommand(0xAE);//Turns display off
  sendCommand(0xD5);//Tells display to expect Clock divide/oscilator frequency information
  sendCommand(0xA0);//Sets Oscillator frequency to 10 and divide ratio to 1
  sendCommand(0xA8);//Tells display to expect Multiplex Ratio Data
  sendCommand(0x3F);//Sets multiplex ratio to 64
  sendCommand(0xD3);//Tells display to expect Display Offset Data
  sendCommand(0x00);//Sets display offset to 0
  sendCommand(0x40);//Sets the display start line to 0
  sendCommand(0xA0);//Sets Segment Remap to map SEG0 to Collum address 0
  sendCommand(0xC0);//Sets COM output scan direction to be from COM0 to COM[N-1]
  sendCommand(0xDA);//Tells display to expect COM Pins hardware config data
  sendCommand(0x12);//Disables COM pin remapping and enables alternative com pins configuration
  sendCommand(0x81);//Tells the SSD1309 to expect a contrast level
  sendCommand(0xDF);//Sets contrast to (Decimal Value +1)/256
  sendCommand(0xD9);//Tells display to expect pre-Charge data
  sendCommand(0x82);//Sets Phase 1 period to 8 DCLK and Phase 2 period to 2 DCLK
  sendCommand(0xDB);//Tells display to expect VCOMH Deselect level data
  sendCommand(0x34);//Sets VCOMh deselect level to 0.78*Vcc
  sendCommand(0xA4);//Sets entire display on
  sendCommand(0xA6);//Sets display to Normal mode (1 is on, 0 is off)
  sendCommand(0x20);//Tells the SSD1309 to expect memory addresing mode information
  sendCommand(0x01);//Sets page addressing mode to Vertical (standard line out by normal display standards.)
  sendCommand(0x21);//Tells the SSD1309 to expect Collum start/end address information
  sendCommand(0x00);//Sets Collum start address to 0
  sendCommand(0x7F);//Sets Collum end address to 127
  sendCommand(0x22);//Tells the SSD1309 to expect Page start/end address information
  sendCommand(0x00);//Sets Page start address to 0
  sendCommand(0x07);//Sets Page end address to 7
  sendCommand(0xAF);//Turns the display on in normal mode
  //
  delay(100);
  pinMode(A0,INPUT_PULLUP);//For button
  pinMode(A1, OUTPUT);//For other arduino - Bass + Arpeggio pin
  pinMode(A2, OUTPUT);//For other arduino - Melody Pin
  setBackground();
  setMines();
  randomSeed(analogRead(A3));
  bno.setExtCrystalUse(true);
  calibrate();
  
  startTime = micros();//Save this for frame timing
}

void loop() {
  frame();
}