#include <ZUNO_SERVO.h>

#define MY_SERIAL Serial

#define SERVO_PIN 12
#define SERVO_ENABLE_PIN 9 

#define BTN_LOCK_PIN 3 
#define BTN_UNLOCK_PIN 4 
#define LED_LOCK_PIN 5 
#define LED_UNLOCK_PIN 6 

// Last saved LED value
byte currentLEDValue;
unsigned long int rfid = 0;

int heartbeat = 0;
unsigned long current_timestamp = 0;
unsigned long heartbeat_timestamp = 0;
unsigned long lock_timestamp = 0;

int lock_status = 0;

byte lastSetDimmer;

ServoController servo(12);

// set up channel
//ZUNO_SETUP_CHANNELS(ZUNO_SWITCH_MULTILEVEL(getter, setter));
//ZUNO_SETUP_DEBUG_MODE(DEBUG_ON);

void lock(int mode){
	if (mode == 1){
		servo.setValue(40);
	}else if (mode == 2){
		servo.setValue(140);
	}
	digitalWrite(SERVO_ENABLE_PIN, HIGH);
	servo.begin();
	lock_timestamp = current_timestamp + 1000;
	lock_status = mode;
}

// the setup routine runs once when you press reset:
void setup() {
	MY_SERIAL.begin(115200);
  	MY_SERIAL.println("Starting...");

  	pinMode(LED_UNLOCK_PIN, OUTPUT); // setup pin as output
	digitalWrite(LED_UNLOCK_PIN, LOW);
  	pinMode(LED_LOCK_PIN, OUTPUT); // setup pin as output
	digitalWrite(LED_LOCK_PIN, LOW);
  	
	pinMode(SERVO_ENABLE_PIN, OUTPUT); // setup pin as output
	digitalWrite(SERVO_ENABLE_PIN, HIGH);
	
	servo.begin();
}
// the loop routine runs over and over again forever:
void loop() {
	current_timestamp = millis();
	
	// Lock disable timeout	
	if (lock_status > 0) {
		if (current_timestamp >= lock_timestamp) {
			servo.end();
			digitalWrite(SERVO_ENABLE_PIN, LOW);
			lock_status = 0;
		}
	}
	// Heartbeat
	if (current_timestamp >= heartbeat_timestamp) {
		if (heartbeat == 1) {
			heartbeat = 0;
			digitalWrite(LED_UNLOCK_PIN, HIGH);
			digitalWrite(LED_LOCK_PIN, HIGH);
			lock(1);
		} else {
			heartbeat = 1;
			//servo.setValue(120);
			digitalWrite(LED_UNLOCK_PIN, LOW);
			digitalWrite(LED_LOCK_PIN, LOW);
			lock(2);
		}
		heartbeat_timestamp = current_timestamp + 5000;
	}
}

void setter(byte value) {
  byte tempVariable;
  // value is a variable, holding a "new value"
  // which came from the controller or other Z-Wave device
  if (value > 99) {
    // by Z-Wave specification, this value can't be more then 99
    value = 99;
  }
  // but the Servo angle scale ranges from 0 to 180
  // we need to prepare our "value" for this new scale
  tempVariable = ((word)value)*180/99;
  // now we set the Servo position
  servo.setValue(tempVariable);
  // let's save our value for the situation, when the controller will ask us about it
  lastSetDimmer = value;
}

// getter function is called once the controller
// or other device in the network asks Z-Uno about
// it's current level
byte getter(void) {
  // return previously saved (in getter()) value
  return lastSetDimmer;
}