//simulation to demonstrate how coding of mail dispensor could be approached
//buttons symbolise outputs from the drone navigation system
//pushing buttons symbolises arriving at respective locations
//leds symbolise releasing latches for respective slots
//green, location 1, latch 1
//red, location 2, latch 2
//ultrasonic distance sensor symbolises proximity sensor

#define PIN_TRIG 3
#define PIN_ECHO 2
#define DEST_ONE 6
#define DEST_TWO 5
#define LATCH_1 4
#define LATCH_2 7
#define RFID_Out 9
#define RFID_In 8
#include<Adafruit_MPU6050.h>
#include<Adafruit_Sensor.h>
#include <Wire.h>
Adafruit_MPU6050 mpu;

int destination_1 = 0;
int destination_2 = 0;
float proximity = 0;

void setup() {
  Serial.begin(115200);
  pinMode(PIN_TRIG, OUTPUT);
  pinMode(PIN_ECHO, INPUT);
  pinMode(DEST_ONE, INPUT_PULLUP);
  pinMode(LATCH_1, OUTPUT);
  pinMode(DEST_TWO, INPUT_PULLUP);
  pinMode(LATCH_2, OUTPUT);
  pinMode(RFID_Out, OUTPUT);
  pinMode(RFID_In, INPUT);

  if (!mpu.begin()) {
 Serial.println("Failed to find MPU6050 chip");
 while (1) {
 delay(10);
 }
 mpu.setAccelerometerRange(MPU6050_RANGE_16_G);
 mpu.setGyroRange(MPU6050_RANGE_250_DEG);
 mpu.setFilterBandwidth(MPU6050_BAND_21_HZ);
 Serial.println("");
 delay(100);
  }
}

void loop() {
  double acc_ax;
  double acc_ay;
  double acc_az;
  destination_1 = digitalRead(DEST_ONE);
  destination_2 = digitalRead(DEST_TWO);
  sensors_event_t a, g, temp;

  if(destination_1 == LOW){ //if destination 1 reached, turn on proximity sensor, send message to RFID

  // Send RFID message:
  digitalWrite(RFID_Out, HIGH);
  int RFID = digitalRead(RFID_In);

  // Measuring Acceleration:
    mpu.getEvent(&a, &g, &temp);
    acc_ax = a.acceleration.x;
    acc_ay = a.acceleration.y;
    acc_az = a.acceleration.z;
    Serial.println(acc_ax);
    Serial.println(acc_ay);
    Serial.println(acc_az);

    // Start a new measurement:
    digitalWrite(PIN_TRIG, HIGH);
    delayMicroseconds(10);
    digitalWrite(PIN_TRIG, LOW);

    // Read the result:
    int duration = pulseIn(PIN_ECHO, HIGH);
    Serial.print("Distance in CM: ");
    proximity = duration / 58;
    Serial.println(proximity);

    delay(100);

    if (proximity < 100 && -0.1 < acc_ax < 0.1 && -0.1 < acc_ay < 0.1 && 9.71 < acc_az < 9.91 && RFID == HIGH ) { // if proximity less than 100 and dispenser not moving, release latch 1
    digitalWrite(LATCH_1, HIGH);
    delay(100);
    }
    else{
    digitalWrite(LATCH_1, LOW);
    }

  }

  else{
    digitalWrite(PIN_TRIG, LOW);
    digitalWrite(LATCH_1, LOW);
  }

  if(destination_2 == LOW){ //if destination 2 reached, turn on proximity sensor
  
    // Start a new measurement:
    digitalWrite(PIN_TRIG, HIGH);
    delayMicroseconds(10);
    digitalWrite(PIN_TRIG, LOW);

    // Read the result:
    int duration = pulseIn(PIN_ECHO, HIGH);
    Serial.print("Distance in CM: ");
    proximity = duration / 58;
    Serial.println(proximity);

    delay(100);
    
    if (proximity < 100 && -0.1 < acc_ax < 0.1 && -0.1 < acc_ay < 0.1 && 9.71 < acc_az < 9.91 ) { // if proximity less than 100, release latch 2
    digitalWrite(LATCH_2, HIGH);
    delay(100);
    }
    else{
    digitalWrite(LATCH_2, LOW);
    }

  }

  else{
    digitalWrite(PIN_TRIG, LOW);
    digitalWrite(LATCH_2, LOW);
  }
  
  
}