#include <Wire.h> //for i2c
#include <Servo.h>

#define drawer_pin 5 //digital input pin
#define servo_pin 10 //pwm servo pin
#define lock_pin A0 //analog input pin hall sensor 
#define address_i2c 8 //address bus 
#define lockthreshold 20 //hall sensor analogue reading threshold
#define closepos  20     //servo position closing (block inserted)
#define openpos   160   //posizione servo opening (block not inserted)


Servo moveLock; // create servo object to control a servo

char todo = 'a';

void setup() {
  Wire.begin(address_i2c);                // join i2c bus with address #8
  Wire.onReceive(receiveEvent); // register event
  Wire.onRequest (requestEvent);
  Serial.begin(115200);           // start serial for output
  Serial.println("Start...");
  pinMode (drawer_pin, INPUT); //drawer pin (switch digital input)
  moveLock.attach(servo_pin);  // attaches the servo on pin 10 to the servo object

  if (drawer() && isLock()) { //initial check to see if it is closed
    closed(); //move servo motor
  } else {
    opened(); //move servo motor
    todo = 'a';
  }
}

void loop() {

  switch (todo) {
    case 'a':
      //drawer is open and must be closed
      if (drawer ()) {
        if (isLock ()) {
          closed();
          todo = '0';       // nothing

        }
      }

      break;

    case 'b':
      //The drawer has just opened because the master has told him to do it. and must not close up
      if ( isLock() == false ) { //do it if the block has been removed
        todo = 'a';  //make sure that you check whether it should close or not
      }
      break;
    default:
      break;
  }

  delay(100);

}
//he does it when something is sent to him from the master
void receiveEvent(int howMany) {
  String cmd = ""; //buffer
  while (Wire.available()) { //if there is at least one byte to read he performs. - 0 <in theory you do not need to put it before wire -
    cmd += char(Wire.read ());  //as long as there is something to read read it
  }
  Serial.print("Command from I2C:\t"); //for print the communication
  Serial.println(cmd);         // print everithing receive
  if (cmd == "closed") {  //master asks to close if he does not do it because it was already closed

    closed ();
  } else if (cmd == "opened") {  //he asks to open if he does not do it because he was already open
    opened ();
    todo = 'b';
  } else {}
}
//when master ask something
void requestEvent () {
  Wire.write(isOpen()) ;
  //is open: value 1 is open - 0 is closed
}


//returns true if the drawers are inserted otherwise false
bool drawer() {
  return digitalRead (drawer_pin);
}

//returns true if it reads the hall sensor
bool isLock () {
  return (abs(analogRead(lock_pin) - 512) > lockthreshold); //the more I'm getting closer, the more it increases
}

//turn servo motor to close
void closed () {
  moveLock.write(closepos);
  Serial.println("closed"); //write on the serial when it's closing, useful for verification

}

//turn servo motor to open
void opened() {
  moveLock.write(openpos);
  Serial.println("opened"); //write on the serial when it's opening, useful for verification
}

//It's open? if is open: true, if is closed: false
bool isOpen () {
  return (moveLock.read() == openpos);
}