#include "HX711.h"
#include "Servo.h"
#include <U8g2lib.h>
#include <Wire.h>
U8G2_SSD1306_128X64_NONAME_F_SW_I2C u8g2(U8G2_R0,SCL,SDA);

HX711 scale;
Servo myservo0;
Servo myservo1;
Servo myservo2;
Servo myservo3;
int echopin = 2;
int trigpin = 3;
int distance = 0;
int flag = 0;
int num = 0;
int type;
String size = "No pressure";

void setup() {
  Serial.begin(9600);
  scale.begin(A1, A0);
  pinMode(echopin, INPUT);
  pinMode(trigpin, OUTPUT);
  myservo0.attach(5);
  myservo1.attach(6);
  myservo2.attach(9);
  myservo3.attach(10);
  u8g2.begin();
  u8g2.enableUTF8Print();
  Serial.print("Hello!");
}

void stress() {
  int stressReading;
  stressReading = scale.get_units()/0.42;
  Serial.print("Analog reading = ");
  Serial.print(stressReading);
  if (stressReading < 10) {
    Serial.println(" - No pressure");
    size = "No pressure";
  } else if (stressReading < 200) {
    Serial.println(" - Small fruit");
    size = "Small fruit";
    delay(2000);
    if (stressReading < 200) {
      flag=1; }
  } else if (stressReading < 500) {
    Serial.println(" - Medium fruit");
    size = "Medium fruit";
    delay(2000);
    if (stressReading < 500) {
      flag=2; }
  } else if (stressReading < 800) {
    Serial.println(" - Large fruit");
    size = "Large fruit";
    delay(2000);
    if (stressReading < 800) {
      flag=3; }
  } else {
    Serial.println(" - Large fruit");
    size = "Large fruit";
  }
}
void reset() {
  myservo0.write(0);
  delay(500);
}
void act1() {
  myservo0.write(90);
  delay(500);
}
void act2() {
  myservo0.write(180);
  delay(500);
}
void detection() {
  digitalWrite(trigpin, LOW);
  digitalWrite(trigpin, HIGH);
  digitalWrite(trigpin, LOW);
  distance = pulseIn(echopin, HIGH) * 0.0172;
  Serial.print("Distance: ");
  Serial.println(distance);
  if (flag = 1 && distance < 50) {
    myservo1.write(0);
    delay(1000);
    myservo1.write(90);
  }
  else {
    myservo1.write(90);
  }
}

void display()
{   
  u8g2.setFont(u8g2_font_ncenB08_tr);
  u8g2.setFontDirection(0);
  u8g2.clearBuffer();
  u8g2.drawStr(0,17,"Fruit type:");
  // u8g2.drawStr(50,17," ");
  u8g2.setCursor(66,17);
  u8g2.print(flag);

  u8g2.drawStr(0,34,"size:");
  // u8g2.setCursor(50,34);
  // u8g2.print(size);
  // u8g2.drawStr(50,34,size);

  u8g2.drawStr(0,50,"distance:");
  u8g2.setCursor(50,50);
  u8g2.print(distance);

  u8g2.sendBuffer();
}

void loop() {
  // Serial.println(scale.get_units()/0.42, 1);
  // delay(1000);
  while(1) {
    reset();
    display();
    delay(1000);
    act1();
    stress();
    if(flag != 0) {
      act2();
      detection();
    }
    delay(1000);
    flag=0;
  }
  delay(1000);
}