#include <Servo.h>
#include <SPI.h>
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include "HX711.h"

HX711 scale1;
#define scale1_DOUT  1
#define scale1_SCK  0
float calibration_factor1 = -440;
int weight1 = 0;

HX711 scale2;
#define scale2_DOUT  3
#define scale2_SCK  2
float calibration_factor2 = -1200;
int weight2 = 0;


HX711 scale3;
#define scale3_DOUT  5
#define scale3_SCK  4
float calibration_factor3 = -1170;
int weight3 = 0;

int ledPin = 12;    // LED connected to digital pin 12

Servo myservo1;  // create servo1 object to control a servo
Servo myservo2;  // create servo2 object to control a servo

int pos = 0;    //  change the servo degrees

#define SCREEN_WIDTH 128 // OLED display width, in pixels
#define SCREEN_HEIGHT 64 // OLED display height, in pixels

#define OLED_RESET     4 // Reset pin # (or -1 if sharing Arduino reset pin)
#define SCREEN_ADDRESS 0x3C ///< See datasheet for Address; 0x3D for 128x64, 0x3C for 128x32
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

#define BMP_HEIGHT   60
#define BMP_WIDTH    60
//'dozo_logo2', 60x60px

void setup() {
 Serial.begin(9600);
  scale1.begin(scale1_DOUT, scale1_SCK);
  scale1.set_scale();
  scale1.tare(); //Reset the scale to 0

  scale2.begin(scale2_DOUT, scale2_SCK);
  scale2.set_scale();
  scale2.tare(); //Reset the scale2 to 0
  
  scale3.begin(scale3_DOUT, scale3_SCK);
  scale3.set_scale();
  scale3.tare(); //Reset the scale3 to 0
  
  myservo1.attach(8);  // attaches the servo1 on pin 8 (PWM) to the servo object
  myservo2.attach(9);  // attaches the servo2 on pin 9 (PWM) to the servo object

  //Set the servos to initial positions
  myservo1.write(90);
  myservo2.write(135);
  
  pinMode(ledPin, OUTPUT);
  digitalWrite(ledPin, HIGH);
 /* 
Wire.swap(1);
  // SSD1306_SWITCHCAPVCC = generate display voltage from 3.3V internally
  if(!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) { // Address 0x3C for 128x32
    Serial.println(F("SSD1306 allocation failed"));
    for(;;); // Don't proceed, loop forever
    }
      
  drawbitmap();
  delay(3000);
  
  //testscrolltext();*/
  }
  
void loop() {
 scale1.set_scale(calibration_factor1); //Adjust to this calibration factor1
 scale2.set_scale(calibration_factor2); //Adjust to this calibration factor2
 scale3.set_scale(calibration_factor3); //Adjust to this calibration factor3
 
 Serial.print("Scale1_Reading: ");
  Serial.print(scale1.get_units(),1);
  Serial.print("g"); 
  Serial.println();
  weight1 = scale1.get_units();
  
  Serial.print("Scale2_Reading: ");
  Serial.print(scale2.get_units(),1);
  Serial.print("g"); 
  Serial.println();
  weight2 = scale2.get_units();
  
  Serial.print("Scale3_Reading: ");
  Serial.print(scale3.get_units(),1);
  Serial.print("g"); 
  Serial.println();
  weight3 = scale3.get_units();


if ((weight1 >= 75) && (weight1 <= 90)) { //If the weight1 is 75-90g
   for (pos = 90; pos >= 0; pos -= 1) { // goes from 90 degrees to 0 degrees
    // in steps of 1 degree
    myservo1.write(pos);              // tell servo1 to go to position in variable 'pos'
    delay(30);                       // waits 30ms for the servo to reach the position
  }

    for (pos = 135; pos >= 55; pos -= 1) { // goes from 135 degrees to 55 degrees
    // in steps of 1 degree
    myservo2.write(pos);              // tell servo2 to go to position in variable 'pos'
    delay(50);                       // waits 50ms for the servo to reach the position
  }
  
  for (pos = 55; pos >= 45; pos -= 1) { // goes from 55 degrees to 45 degrees
    // in steps of 1 degree
    myservo2.write(pos);              // tell servo2 to go to position in variable 'pos'
    delay(300);                       // waits 15ms for the servo to reach the position
  }
  
  for (pos = 45; pos <= 135; pos += 1) { // goes from 45 degrees to 135 degrees
    // in steps of 1 degree
    myservo2.write(pos);              // tell servo2 to go to position in variable 'pos'
    delay(50);                       // waits 50ms for the servo to reach the position
  }
  
  for (pos = 0; pos <= 90; pos += 1) { // goes from 0 degrees to 90 degrees
    // in steps of 1 degree
    myservo1.write(pos);              // tell servo1 to go to position in variable 'pos'
    delay(30);                       // waits 30ms for the servo to reach the position
  }
  }

   if ((weight2 <= -75) && (weight2 >= -90)) { //If the weight1 is -75- -90g
     for (pos = 135; pos >= 55; pos -= 1) { // goes from 135 degrees to 55 degrees
    // in steps of 1 degree
    myservo2.write(pos);              // tell servo2 to go to position in variable 'pos'
    delay(50);                       // waits 50ms for the servo to reach the position
  }
  
  for (pos = 55; pos >= 45; pos -= 1) { // goes from 55 degrees to 45 degrees
    // in steps of 1 degree
    myservo2.write(pos);              // tell servo2 to go to position in variable 'pos'
    delay(300);                       // waits 300ms for the servo to reach the position
  }
  
  for (pos = 45; pos <= 135; pos += 1) { // goes from 45 degrees to 135 degrees
    // in steps of 1 degree
    myservo2.write(pos);              // tell servo2 to go to position in variable 'pos'
    delay(50);                       // waits 50ms for the servo to reach the position
  }
   }

     if ((weight3 <= -75) && (weight3 >= -90)) { //If the weight1 is -75- -90g
   for (pos = 90; pos <= 180; pos += 1) { // goes from 90 degrees to 180 degrees
    // in steps of 1 degree
    myservo1.write(pos);              // tell servo1 to go to position in variable 'pos'
    delay(30);                       // waits 30ms for the servo to reach the position
  }

   for (pos = 135; pos >= 55; pos -= 1) { // goes from 135 degrees to 180 degrees
    // in steps of 1 degree
    myservo2.write(pos);              // tell servo2 to go to position in variable 'pos'
    delay(50);                       // waits 50ms for the servo to reach the position
  }
  
  for (pos = 55; pos >= 45; pos -= 1) { // goes from 55 degrees to 45 degrees
    // in steps of 1 degree
    myservo2.write(pos);              // tell servo2 to go to position in variable 'pos'
    delay(300);                       // waits 300ms for the servo to reach the position
  }
 
  for (pos = 45; pos <= 135; pos += 1) { // goes from 45 degrees to 135 degrees
    // in steps of 1 degree
    myservo2.write(pos);              // tell servo2 to go to position in variable 'pos'
    delay(50);                       // waits 50ms for the servo to reach the position
  }
  
  for (pos =180; pos >= 90; pos -= 1) { // goes from 180 degrees to 90 degrees
    // in steps of 1 degree
    myservo1.write(pos);              // tell servo1 to go to position in variable 'pos'
    delay(30);                       // waits 30ms for the servo to reach the position
  }
   }  
   }
 
/*
const unsigned char myBitmap [] PROGMEM = {
  0xff, 0xff, 0xff, 0xf8, 0x0f, 0xff, 0xff, 0xf0, 0xff, 0xff, 0xff, 0x1f, 0xfc, 0x7f, 0xff, 0xf0, 
  0xff, 0xff, 0xf9, 0xff, 0xff, 0xcf, 0xff, 0xf0, 0xff, 0xff, 0xe7, 0x80, 0x00, 0xf3, 0xff, 0xf0, 
  0xff, 0xff, 0x9c, 0x00, 0x00, 0x1d, 0xff, 0xf0, 0xff, 0xfe, 0x60, 0x00, 0x00, 0x06, 0x7f, 0xf0, 
  0xff, 0xfd, 0x80, 0x00, 0x00, 0x01, 0x3f, 0xf0, 0xff, 0xf3, 0x00, 0x07, 0xf0, 0x00, 0xdf, 0xf0, 
  0xff, 0xe4, 0x00, 0x78, 0x04, 0x00, 0x6f, 0xf0, 0xff, 0xc8, 0x01, 0x80, 0x00, 0xc0, 0x17, 0xf0, 
  0xff, 0x90, 0x04, 0x0f, 0xf8, 0x20, 0x1b, 0xf0, 0xff, 0x20, 0x10, 0x7f, 0xff, 0x08, 0x09, 0xf0, 
  0xfe, 0x40, 0x21, 0xff, 0xff, 0xc4, 0x05, 0xf0, 0xfe, 0x80, 0xc7, 0xff, 0xff, 0xe2, 0x02, 0xf0, 
  0xfd, 0x01, 0x0f, 0xff, 0xff, 0xf1, 0x02, 0xf0, 0xf9, 0x02, 0x1f, 0xfc, 0x3f, 0xf8, 0x81, 0x70, 
  0xfa, 0x06, 0x7f, 0xf9, 0x3f, 0xfc, 0x40, 0x70, 0xf4, 0x0c, 0x7f, 0xf2, 0x3f, 0xfe, 0x00, 0xb0, 
  0xf4, 0x08, 0xff, 0xe4, 0x1f, 0xff, 0x20, 0xb0, 0xf8, 0x19, 0xff, 0xc8, 0x0f, 0xff, 0x00, 0x70, 
  0xe8, 0x33, 0xff, 0xc0, 0x07, 0xff, 0x90, 0x10, 0xe8, 0x33, 0xff, 0xf0, 0x01, 0xff, 0x90, 0x50, 
  0xf0, 0x27, 0xff, 0xb8, 0x00, 0x7f, 0xc8, 0x10, 0xd0, 0x67, 0xff, 0x3c, 0x00, 0x1f, 0xc8, 0x30, 
  0xd0, 0x6f, 0xff, 0x7e, 0x00, 0x07, 0xc8, 0x10, 0xd0, 0x4f, 0xff, 0x7e, 0x00, 0x03, 0xec, 0x20, 
  0xf0, 0xcf, 0xff, 0xfe, 0x00, 0x03, 0xec, 0x20, 0xe0, 0xcf, 0xff, 0xff, 0x00, 0x01, 0xe4, 0x00, 
  0xe0, 0xcf, 0xff, 0xff, 0x00, 0x00, 0xe4, 0x00, 0xe0, 0xdf, 0xff, 0xff, 0x00, 0x00, 0xe4, 0x00, 
  0xe0, 0xdf, 0xff, 0xff, 0x00, 0x00, 0xe4, 0x00, 0xe0, 0xcf, 0xff, 0xff, 0x00, 0x00, 0xe4, 0x00, 
  0xe0, 0xcf, 0xf0, 0x1f, 0x00, 0x00, 0xe4, 0x00, 0xe0, 0xcf, 0xcf, 0xe7, 0x00, 0x00, 0xec, 0x20, 
  0xd0, 0xcf, 0xbf, 0xc7, 0x00, 0x00, 0xec, 0x20, 0xd0, 0x6f, 0x80, 0x07, 0x00, 0x00, 0xc8, 0x10, 
  0xd0, 0x67, 0x80, 0x07, 0x80, 0x00, 0xc8, 0x10, 0xf0, 0x67, 0xc0, 0x0f, 0x80, 0x00, 0xc8, 0x10, 
  0xe8, 0x33, 0xc0, 0x0f, 0xc0, 0x01, 0x90, 0x10, 0xe8, 0x33, 0xe0, 0x1f, 0xc0, 0x03, 0x90, 0x10, 
  0xf8, 0x19, 0xf8, 0x3f, 0xf0, 0x07, 0x30, 0x70, 0xf4, 0x08, 0xfc, 0x3f, 0xfc, 0x1f, 0x20, 0x30, 
  0xf4, 0x0c, 0x7f, 0xff, 0xff, 0xfe, 0x60, 0x30, 0xfa, 0x06, 0x3f, 0xff, 0xff, 0xfc, 0x40, 0x70, 
  0xfc, 0x03, 0x1f, 0xff, 0xff, 0xf8, 0x81, 0x70, 0xfd, 0x01, 0x8f, 0xff, 0xff, 0xf1, 0x00, 0xf0, 
  0xfe, 0x80, 0xc7, 0xff, 0xff, 0xe2, 0x02, 0xf0, 0xff, 0x40, 0x61, 0xff, 0xff, 0xc4, 0x05, 0xf0, 
  0xff, 0x20, 0x18, 0x7f, 0xff, 0x08, 0x03, 0xf0, 0xff, 0x90, 0x0e, 0x0f, 0xf8, 0x30, 0x03, 0xf0, 
  0xff, 0xc8, 0x03, 0x80, 0x00, 0xc0, 0x07, 0xf0, 0xff, 0xe4, 0x00, 0x7c, 0x1f, 0x00, 0x0f, 0xf0, 
  0xff, 0xfa, 0x00, 0x0f, 0xf8, 0x00, 0x9f, 0xf0, 0xff, 0xfc, 0x80, 0x00, 0x00, 0x01, 0x3f, 0xf0, 
  0xff, 0xfe, 0x60, 0x00, 0x00, 0x06, 0x7f, 0xf0, 0xff, 0xff, 0x98, 0x00, 0x00, 0x19, 0xff, 0xf0, 
  0xff, 0xff, 0xe7, 0x00, 0x00, 0x67, 0xff, 0xf0, 0xff, 0xff, 0xf8, 0xfc, 0x1f, 0x9f, 0xff, 0xf0, 
  0xff, 0xff, 0xff, 0x8f, 0xf8, 0xff, 0xff, 0xf0, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xf0
};
void drawbitmap() {
  display.clearDisplay();
  display.drawBitmap(
    34,
    0,
    myBitmap, BMP_WIDTH, BMP_HEIGHT, 1);
  display.display();
  delay(5000);
}

void testscrolltext() {
  display.clearDisplay();
  display.setTextSize(3);
  display.setTextColor(WHITE);
  display.setCursor(0, 0);
  display.println("Dozo!");
  display.println("Dozo!");
  display.display();
  display.startscrollright(0x00, 0x07);     delay(3000);
  display.stopscroll();
}
*/