 
   // check if it's time to read data and update the filter
   microsNow = micros();
-  if (microsNow - microsPrevious >= microsPerReading) { // This condition is taken from the tutorial code -- it ensures updates happen at the right time
-    float difference = measureYaw() - 180; // Normalize the measurement so that it starts from 0, since the initial angle is 180
-    doorAngle = abs(difference); // Note that the abs function cannot have other operations inside of it.  This is apparently a problem with Arduino.  This line and the previous one have to be two separate operations, or this will bug up.
-
-    if (doorAngle < OPEN_ANGLE) { // Door is closed
+  if (microsNow - microsPrevious >= microsPerReading) { // This condition is taken from the tutorial code -- it ensures updates happen at the right time --??? is this for IMU delay
+    
+    // Checks the magnetic switch
+    if (analogRead(SWITCH_PIN) > 800) {            // can tune threshold depending on circuit. currently closed switch = (analogRead output > 800)
+      isMagSwitchOpen   = false;                   // switch is CLOSED, NOT open
+      isMagSwitchClosed = true;
+    }  else if (analogRead(SWITCH_PIN) < 800) {
+      isMagSwitchOpen   = true;                    // switch is OPEN, NOT closed
+      isMagSwitchClosed = false;
+    }
+
+    if (isMagSwitchClosed) { // Door is closed
       closeDoor();
       printIfDebug("Door not open");
-    } else if (microsNow - microsLastRead < microsBetweenReads) { // Door is open, a movement was recently detected, and we are waiting between movements; do not double count or re-record the last passing time because the person may still be passing through.
+      readyToBeOpened = true;
+      
+    } else if ((microsNow - microsLastRead < microsBetweenReads) and isMagSwitchOpen) { // Door is open, a movement was recently detected, and we are waiting between movements; do not double count or re-record the last passing time because the person may still be passing through.
       printIfDebug("Movement Detected");
-    } else { // Door is open, but a complete movement has not been discovered recently.
+      
+    } else if (isMagSwitchOpen) { // Door is open, but a complete movement has not been discovered recently.
+      if (readyToBeOpened) {
+        baselineYaw = measureYaw();
+        readyToBeOpened = false;
+      }
+      float currentYaw = measureYaw();
+      float absDifference = abs(currentYaw - baselineYaw);
+      // for now... i will fix with better logic later... probably account for directionality using a similar button system
+      doorAngle=absDifference;      
+
+      
       openDoor();
       ping_1 = sonar_1.ping_cm();
       ping_2 = sonar_2.ping_cm();

#include <MadgwickAHRS.h>
#include <NewPing.h>
#include <Wire.h>
#include "rgb_lcd.h"

// parameters indicated with a * should be tuned to fit the door.

// ULTRASONIC
#define TRIGGER_PIN_1  12  // Arduino pin tied to trigger pin on the first ultrasonic sensor.
#define ECHO_PIN_1     11  // Arduino pin tied to echo pin on the first ultrasonic sensor.
#define TRIGGER_PIN_2  10  // Arduino pin tied to trigger pin on the second ultrasonic sensor.
#define ECHO_PIN_2     9  // Arduino pin tied to echo pin on the second ultrasonic sensor.

#define DETECTION_THRESH 25 // Distance cutoff for passing (< DETECTION_THRESH is considered to be a person in proximity)
#define OPEN_ANGLE 20       // Angle where door is considered open


  // configure LCD
  lcd.begin(16, 2);
  lcd.setBacklight(0x7);    // sets backlight to brightest setting (white)
  lcd.print("Population: 0   ");

  // establish pinMode for magnetic contact switch
  pinMode(SWITCH_PIN, INPUT); // if analogRead pulls high, then switch is closed. else, door is open.

  // start the IMU and filter
  CurieIMU.begin();
