#include <basicMPU6050.h>

//To be ran on Arduino IDE
#include <Wire.h>
//Make sure necessary packages are downloaded
#include <Adafruit_SSD1306.h>


//Make Sure Address is Correct
#define OLED_ADDRESS 0x3C

enum Face{
  LOOKING,
  STARING,
  BLINKING,
  WINKING,
  LOOKING_LEFT,
  LOOKING_RIGHT,
  MAD,
  SMILING,
  FAINTING,
  LOVING
};

enum Eye {
  NORMAL, 
  WIDE,
  WINK,
  HAPPY,
  MAD_LEFT,
  MAD_RIGHT,
  DEAD,
  HEART
};

enum Mouth {
  SMILE,
  FLAT,
  FROWN,
  OPEN
};

Adafruit_SSD1306 display(128, 64, &Wire, OLED_ADDRESS);
// Create instance
basicMPU6050<> imu;

int eyeSize = 18;
int eyeSpace = 70;
int mouthSpace = 20;

int mouthWidth = 16;
int mouthHeight = 3;
int frame = 0;
int faceX = 64;
int faceY = 29;
Face currentFace = LOOKING;
int cooldown = 0;


void setup() {
  // Set registers - Always required
  imu.setup();

  // Initial calibration of gyro
  imu.setBias();

    Serial.begin(115200);



  display.begin(SSD1306_SWITCHCAPVCC, OLED_ADDRESS);


  drawIntro();
  display.clearDisplay();
}

void drawIntro() {
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(WHITE);
  display.setCursor(33, 23);
  display.println("let's");
  display.display();
  delay(300);
  display.clearDisplay();

  display.setCursor(2, 22);
  display.setTextSize(3);
  display.println("Fuckin'");
  display.display();
  delay(300);
  display.clearDisplay();

  display.setCursor(21, 18);
  display.setTextSize(4);
  display.println("GO!!");
  display.display();
  delay(1000);
}

void loop() {

    /* Get new sensor events with the readings */
 imu.updateBias();

  display.clearDisplay();
  if(imu.az() > -2){
    cooldown = 20;
    frame = 0;
    currentFace = FAINTING;
  }
  else if(imu.az() < 2) {
    cooldown = 0;
    frame = 0;
    currentFace = STARING;
  }
  else if(imu.ay() > 20 || imu.ay() < -2) {
    cooldown = 20;
    frame = 0;
    currentFace = MAD;
  }
  else if(imu.ax() > 0.20) {
    cooldown = 0;
    frame = 0;
    currentFace = LOOKING_LEFT;
  }
  else if(imu.ax() < -0.20) {
    cooldown = 0;
    frame = 0;
    currentFace = LOOKING_RIGHT;
  }
  else if(cooldown <= 0) {
    frame++;
    if(frame > 102) {
      
      frame = 0;
    }
    else if(frame > 100) {
      currentFace = BLINKING;
    }
    else if(frame == 1) {
      int randomNum = random(100);
      if(randomNum > 95) {
        currentFace = LOVING;
      }
      else if(randomNum > 2) {
        currentFace = SMILING;
      }
      else {
        currentFace = LOOKING;
      }
    }
  }
  else {
    cooldown--;
  }

  drawFace(faceX, faceY, currentFace);
  display.display();

  delay(10);
}

void drawFace(int x, int y, Face face) {
  x = x + (imu.ax()*.8);
  y = y + (imu.ay()*.4);
  switch(face) {
    case LOOKING:
      drawEye(x-eyeSpace/2, y, NORMAL);
      drawEye(x+eyeSpace/2, y, NORMAL);
      drawMouth(x, y+mouthSpace, SMILE);
      break;
    case LOVING:
      drawEye(x-eyeSpace/2, y, HEART);
      drawEye(x+eyeSpace/2, y, HEART);
      drawMouth(x, y+mouthSpace, SMILE);
      break;
    case STARING:
      drawEye(x-eyeSpace/2, y, WIDE);
      drawEye(x+eyeSpace/2, y, WIDE);
      drawMouth(x, y+mouthSpace, OPEN);
      break;
    case BLINKING:
      drawEye(x-eyeSpace/2, y, WINK);
      drawEye(x+eyeSpace/2, y, WINK);
      drawMouth(x, y+mouthSpace, SMILE);
      break;
    case WINKING:
      drawEye(x-eyeSpace/2, y, HAPPY);
      drawEye(x+eyeSpace/2, y, WINK);
      drawMouth(x, y+mouthSpace, SMILE);
      break;
    case LOOKING_LEFT:
      drawEye(x-eyeSpace/2, y, MAD_LEFT);
      drawEye(x+eyeSpace/2, y, MAD_LEFT);
      drawMouth(x, y+mouthSpace, FLAT);
      break;
    case LOOKING_RIGHT:
      drawEye(x-eyeSpace/2, y, MAD_RIGHT);
      drawEye(x+eyeSpace/2, y, MAD_RIGHT);
      drawMouth(x, y+mouthSpace, FLAT);
      break;
    case MAD:
      drawEye(x-eyeSpace/2, y, MAD_RIGHT);
      drawEye(x+eyeSpace/2, y, MAD_LEFT);
      drawMouth(x, y+mouthSpace, OPEN);
      break;
    case SMILING:
      drawEye(x-eyeSpace/2, y, HAPPY);
      drawEye(x+eyeSpace/2, y, HAPPY);
      drawMouth(x, y+mouthSpace, SMILE);
      break;
    case FAINTING:
      drawEye(x-eyeSpace/2, y, DEAD);
      drawEye(x+eyeSpace/2, y, DEAD);
      drawMouth(x, y+mouthSpace, FROWN);
      break;
  }
}


void drawEye(int x, int y, Eye style) {
  int size = eyeSize;
  switch(style) {
    case NORMAL:
      display.fillCircle(x, y, size/2, WHITE);
      break;

    case WIDE:
      size = eyeSize * ((imu.az()<15 ? imu.az() : 15) /10);
      display.fillCircle(x, y, size/2, WHITE);
      display.fillCircle(x, y, (size*.7)/2, BLACK);
      break;

    case WINK:
      size = eyeSize * .9;
      display.fillRect(x-size/2, y-1, size, 3, WHITE);
      display.fillCircle(x-size/2, y+1, 1, WHITE);
      display.fillCircle(x+size/2, y+1, 1, WHITE);
      break;

    case HAPPY:
      display.drawLine(x-size/2, y+4, x, y-3, WHITE);
      display.drawLine(x-size/2, y+3, x, y-4, WHITE);
      display.drawLine(x-size/2, y+2, x, y-5, WHITE);
      display.drawLine(x-size/2, y+1, x, y-6, WHITE);

      display.drawLine(x, y-3, x+size/2, y+4, WHITE);
      display.drawLine(x, y-4, x+size/2, y+3, WHITE);
      display.drawLine(x, y-5, x+size/2, y+2, WHITE);
      display.drawLine(x, y-6, x+size/2, y+1, WHITE);
      break;


    case MAD_LEFT:
      size = eyeSize * .85;
      x-=2;
      display.drawLine(x+size/2, y-size/2-1, x-size/2, y-1, WHITE);
      display.drawLine(x+size/2, y-size/2, x-size/2, y, WHITE);
      display.drawLine(x+size/2, y-size/2+1, x-size/2, y+1, WHITE);

      display.drawLine(x-size/2, y-1, x+size/2, y+size/2-1, WHITE);
      display.drawLine(x-size/2, y, x+size/2, y+size/2, WHITE);
      display.drawLine(x-size/2, y+1, x+size/2, y+size/2+1, WHITE);
      break;

    case MAD_RIGHT:
      size = eyeSize * .85;
      x+=2;
      display.drawLine(x-size/2, y-size/2-1, x+size/2, y-1, WHITE);
      display.drawLine(x-size/2, y-size/2, x+size/2, y, WHITE);
      display.drawLine(x-size/2, y-size/2+1, x+size/2, y+1, WHITE);

      display.drawLine(x+size/2, y-1, x-size/2, y+size/2-1, WHITE);
      display.drawLine(x+size/2, y, x-size/2, y+size/2, WHITE);
      display.drawLine(x+size/2, y+1, x-size/2, y+size/2+1, WHITE);
      break;
    
    case DEAD:
      size = eyeSize * .85;
      display.drawLine(x-size/2, y-size/2+1, x+size/2, y+size/2-3, WHITE);
      display.drawLine(x-size/2, y-size/2+2, x+size/2, y+size/2-2, WHITE);
      display.drawLine(x-size/2, y-size/2+3, x+size/2, y+size/2-1, WHITE);

      display.drawLine(x-size/2, y+size/2-3, x+size/2, y-size/2+1, WHITE);
      display.drawLine(x-size/2, y+size/2-2, x+size/2, y-size/2+2, WHITE);
      display.drawLine(x-size/2, y+size/2-1, x+size/2, y-size/2+3, WHITE);
      break;
    case HEART: 
      display.fillTriangle(x-size/2, y, x+size/2, y, x, y+size/2, WHITE);
      display.fillCircle(x-size/4-1, y-size/4, size/3, WHITE);
      display.fillCircle(x+size/4+1, y-size/4, size/3, WHITE);
  }
}


void drawMouth(int x, int y, Mouth mouth) {
  switch(mouth) {
    case SMILE:
      display.fillRect(x-mouthWidth/2, y, mouthWidth, mouthHeight, WHITE);
      display.fillCircle(x-mouthWidth/2, y, 1, WHITE);
      display.fillCircle(x+mouthWidth/2, y, 1, WHITE);
      break;
    case FLAT:
      display.fillRect(x-mouthWidth/2, y, mouthWidth, mouthHeight, WHITE);
      display.fillCircle(x-mouthWidth/2, y+1, 1, WHITE);
      display.fillCircle(x+mouthWidth/2, y+1, 1, WHITE);
      break;
    case FROWN:
      display.fillRect(x-mouthWidth/2, y, mouthWidth, mouthHeight, WHITE);
      display.fillCircle(x-mouthWidth/2, y+2, 1, WHITE);
      display.fillCircle(x+mouthWidth/2, y+2, 1, WHITE);
      break;
    case OPEN:
      display.fillCircle(x-2, y, mouthWidth*.5, WHITE);
      display.fillCircle(x-1, y, mouthWidth*.5, WHITE);
      display.fillCircle(x, y, mouthWidth*.5, WHITE);
      display.fillCircle(x+1, y, mouthWidth*.5, WHITE);
      display.fillCircle(x+2, y, mouthWidth*.5, WHITE);
      break;
  }
}