#include <Wire.h>
#include <HMC58X3.h>

HMC58X3 magn;
int opened=0;
int outerSensor=0;
int innerSensor=0;
int countIn=0;
int countOut=0;
int inSense=0;
int outSense=0;
int cycleLock=0;
int entering=0;
int exiting=0;
int timeout=0;
int delayLoop=0;
int x,y,z;

void setup(void) {
  Serial.begin(9600);
  Wire.begin();
  pinMode(2, OUTPUT);     
  pinMode(3, OUTPUT);
  pinMode(10, OUTPUT);  
  magn.init(true); 
}

void loop() { 
  delay(10);
  // Only read the sensor every 1/2 second, or the Arduino crashes for some reason
  if (delayLoop == 0) {
    magn.getRaw(&x,&y,&z);
    Serial.println(y);
    delayLoop++;
  } else if (delayLoop > 50) {
    delayLoop=0;
  } else {
    delayLoop++;
  }

  // If puck in open position open immediately
  if (y < -150) {
    digitalWrite(10, LOW);
    if (opened == 0) {
      openDoor();
      opened=1;
    }  
    Serial.println("Locked open");
  // Same for close
  } else if (y > 50) {
    digitalWrite(10, LOW);
    if (opened == 1) {
      closeDoor();
      opened=0;
    }  
    Serial.println("Locked closed");
  // If puck isn't around then go into auto mode, only if door last state was closed though
  } else {
    if (opened == 0) {
      goSensor();
      digitalWrite(10 , HIGH);
    }
  }
}

  void goSensor() {
  outerSensor=analogRead(A1);
  innerSensor=analogRead(A0); 
  
  // If outer sensor detects something, wait for 20 positives before setting flag
  if (outerSensor > 218) {
     countOut++;
     if (countOut > 20) {
       outSense=1;
     } 
  } else {
    countOut=0;
  }
  
  // Same again for the inside sensor, but with different sensitivity
  if (innerSensor > 260) {
     countIn++;
     if (countIn > 20) {
       inSense=1;
     } 
  } else {
    countIn=0;
  }
  
  if (timeout > 0) {
    timeout--;
  }
  
  // If the sensor is triggered for the first time, open the door
  if ((outSense == 1) && (entering == 0) && (exiting == 0) && (timeout ==  0)) {
     entering=1;
     openDoor();
  } else if ((inSense == 1) && (exiting == 0) && (entering == 0) && (timeout == 0)) {
    exiting=1;
    openDoor();
  }
 
 
  // This one is for entering from outside to inside - wait until the inner sensor is triggered before closing...
  if (entering > 0) {  
    entering++;
    if (inSense == 1) {
      closeDoor();
      timeout=200;
      entering=0;
      exiting=0;
    } 
  }
 
  // Or until the timeout is reached
  if (entering > 1000) {
    entering=0;
    exiting=0;
    closeDoor();
    timeout=200;
  }

 // Same again but the other way around...
 if (exiting > 0) {  
    exiting++;
    if (outSense == 1) {
      closeDoor();
      timeout=200;
      exiting=0;
      entering=0;
    } 
  }
 
  if (exiting > 1000) {
    exiting=0;
    entering=0;
    closeDoor();
    timeout=200;
  }
 
  inSense=0;
  outSense=0;

}

// The open and close door routines - these trigger a relay which in turn supplies 24V to the pneumatic solenoids...timings are slightly different and tuned to the door characteristics
void openDoor() {
   digitalWrite(3, HIGH); delay(1100); digitalWrite(3, LOW);
}

void closeDoor() {
   digitalWrite(2, HIGH); delay(750); digitalWrite(2, LOW);
}