#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include "HX711.h"
#include <MPU6050_light.h>

#define DOUT_PIN 2 
#define CLK_PIN 3 

#define OLED_RESET -1
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64

#define REST_INTERVAL 10000 
#define GRIP_THRESHOLD 100.0
#define ANGLE_THRESHOLD 45 

Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET, 4, 5); 
HX711 scale;

MPU6050 mpu(Wire);

void setup()
{
  Serial.begin(9600);

  if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C))
  {
    Serial.println(F("SSD1306 allocation failed"));
    for (;;)
      ;
  }

  display.display();
  delay(2000);
  display.clearDisplay();
  scale.begin(DOUT_PIN, CLK_PIN);
  scale.set_scale(-0.42);
  scale.tare(); 
  Wire.begin();
  mpu.begin(); 
}


void loop() {


  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(0, 0);
  display.println("Rest");
  display.display();
  delay(REST_INTERVAL);

  
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(0, 0);
  display.println("Start to Grip");
  display.display();
  delay(3000);

  float load = scale.get_units();
  mpu.update();
  
  float angleX = mpu.getAngleX();
  float angleY = mpu.getAngleY();
  float angleZ = mpu.getAngleZ();

  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(0, 0);
  if ((abs(load) >= GRIP_THRESHOLD) && ((abs(angleX) > ANGLE_THRESHOLD) || (abs(angleY) > ANGLE_THRESHOLD) || (abs(angleZ) > ANGLE_THRESHOLD))) {
    display.println("Good Grip");
  } else {
    display.println("Grip More");
  }
  display.display();

  delay(REST_INTERVAL);


}