#include <ZUNO_SERVO.h>

#define RFID Serial

#define SERVO_PIN 12
const int SERVO_ENABLE_PIN = 9;

const int SENSOR_LOCK_PIN = 2;
const int SENSOR_HANDLE_PIN = 1;
const int SENSOR_DISABLE_PIN = 0;
const int SENSOR_CLOSE_PIN = 10;

const int BTN_LOCK_PIN = 3;
const int BTN_UNLOCK_PIN = 4;
const int LED_LOCK_PIN = 5;
const int LED_UNLOCK_PIN = 6;

#define SERVO_DISABLE 0
#define SERVO_LOCK 1
#define SERVO_UNLOCK 2
#define SERVO_CENTER 3

#define SOURCE_EXTERNAL 0
#define SOURCE_RFID 1
#define SOURCE_ZWAVE 2
#define SOURCE_BUTTONS 3
#define SOURCE_SYSTEM 4

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

#define SIGNAL_DEBOUNCE_CONSTANT 30

bool sensor_lock = false;
bool sensor_handle = false;
bool sensor_disable = false;
bool sensor_close = false;

int sensor_lock_prev_state = HIGH;
int sensor_lock_current_state = LOW;
int sensor_lock_debounce = 0;
int sensor_handle_prev_state = HIGH;
int sensor_handle_current_state = LOW;
int sensor_handle_debounce = 0;
int sensor_disable_prev_state = HIGH;
int sensor_disable_current_state = LOW;
int sensor_disable_debounce = 0;
int sensor_close_prev_state = HIGH;
int sensor_close_current_state = LOW;
int sensor_close_debounce = 0;

bool btn_lock = false;
bool btn_unlock = false;

int btn_lock_prev_state = HIGH;
int btn_lock_current_state = LOW;
int btn_lock_debounce = 0;
int btn_unlock_prev_state = HIGH;
int btn_unlock_current_state = LOW;
int btn_unlock_debounce = 0;

int activity_flash = 0;
int servo_status = 0;
int lock_status = 0;
int source_status = 0;

unsigned long current_timestamp = 0;
unsigned long activity_timestamp = 0;
unsigned long servo_timestamp = 0;

ServoController servo(12);

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

void set_servo(int mode, int source){
	if (sensor_handle == true && sensor_disable == false && sensor_close == true){
		if (mode == SERVO_LOCK){
			servo.setValue(160+2);
			lock_status = SERVO_LOCK;
		}
		else if (mode == SERVO_UNLOCK){
			servo.setValue(20+2);
			lock_status = SERVO_UNLOCK;
		}
		else if (mode == SERVO_CENTER){
			servo.setValue(90+2);
		}
		if (mode == SERVO_DISABLE){
			digitalWrite(SERVO_ENABLE_PIN, LOW);
		}
		else{
			servo.begin();
			digitalWrite(SERVO_ENABLE_PIN, HIGH);
			servo_timestamp = current_timestamp + 700;
		}
		servo_status = mode;
		source_status = source;
	}
}

void update_buttons(){
	sensor_lock_current_state = digitalRead(SENSOR_LOCK_PIN);
	sensor_handle_current_state = digitalRead(SENSOR_HANDLE_PIN);
	sensor_disable_current_state = digitalRead(SENSOR_DISABLE_PIN);
	sensor_close_current_state = digitalRead(SENSOR_CLOSE_PIN);
	
	btn_lock_current_state = digitalRead(BTN_LOCK_PIN);
	btn_unlock_current_state = digitalRead(BTN_UNLOCK_PIN);
	
	// Debounce
	if (sensor_lock_current_state != sensor_lock_prev_state) {
		if (++sensor_lock_debounce == SIGNAL_DEBOUNCE_CONSTANT) {
			if (sensor_lock_current_state == LOW) {
				sensor_lock = false;
			}
			else{
				sensor_lock = true;
			}
			sensor_lock_prev_state = sensor_lock_current_state;
			sensor_lock_debounce = 0;
		}
	} else {
		sensor_lock_debounce = 0;
	}
	if (sensor_handle_current_state != sensor_handle_prev_state) {
		if (++sensor_handle_debounce == SIGNAL_DEBOUNCE_CONSTANT) {
			if (sensor_handle_current_state == LOW) {
				sensor_handle = true;
			}
			else{
				sensor_handle = false;
			}
			sensor_handle_prev_state = sensor_handle_current_state;
			sensor_handle_debounce = 0;
		}
	} else {
		sensor_handle_debounce = 0;
	}
	if (sensor_disable_current_state != sensor_disable_prev_state) {
		if (++sensor_disable_debounce == SIGNAL_DEBOUNCE_CONSTANT) {
			if (sensor_disable_current_state == LOW) {
				sensor_disable = false;
			}
			else{
				sensor_disable = true;
			}
			sensor_disable_prev_state = sensor_disable_current_state;
			sensor_disable_debounce = 0;
		}
	} else {
		sensor_disable_debounce = 0;
	}
	if (sensor_close_current_state != sensor_close_prev_state) {
		if (++sensor_close_debounce == SIGNAL_DEBOUNCE_CONSTANT) {
			if (sensor_close_current_state == LOW) {
				sensor_close = false;
			}
			else{
				sensor_close = true;
			}
			sensor_close_prev_state = sensor_close_current_state;
			sensor_close_debounce = 0;
		}
	} else {
		sensor_close_debounce = 0;
	}
	if (btn_lock_current_state != btn_lock_prev_state) {
		if (++btn_lock_debounce == SIGNAL_DEBOUNCE_CONSTANT) {
			if (btn_lock_current_state == LOW) {
				if (servo_status == SERVO_DISABLE){
					btn_lock = true;
				}
			}
			btn_lock_prev_state = btn_lock_current_state;
			btn_lock_debounce = 0;
		}
	} else {
		btn_lock_debounce = 0;
	}
	if (btn_unlock_current_state != btn_unlock_prev_state) {
		if (++btn_unlock_debounce == SIGNAL_DEBOUNCE_CONSTANT) {
			if (btn_unlock_current_state == LOW) {
				if (servo_status == SERVO_DISABLE){
					btn_unlock = true;
				}
			}
			btn_unlock_prev_state = btn_unlock_current_state;
			btn_unlock_debounce = 0;
		}
	} else {
		btn_unlock_debounce = 0;
	}
}

// the setup routine runs once when you press reset:
void setup() {
	RFID.begin(9600);

	pinMode(SENSOR_LOCK_PIN, INPUT_PULLUP);
	pinMode(SENSOR_HANDLE_PIN, INPUT_PULLUP);
	pinMode(SENSOR_DISABLE_PIN, INPUT_PULLUP);
	pinMode(SENSOR_CLOSE_PIN, INPUT_PULLUP);
	pinMode(BTN_LOCK_PIN, INPUT_PULLUP);
	pinMode(BTN_UNLOCK_PIN, INPUT_PULLUP);

  	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, LOW);
	
	set_servo(SERVO_CENTER, SOURCE_SYSTEM);	
}

// the loop routine runs over and over again forever:
void loop() {
	current_timestamp = millis();
	
	update_buttons();


	// Check button status
	if (btn_lock == true){
		set_servo(SERVO_LOCK, SOURCE_BUTTONS);
		btn_lock = false;			
		btn_unlock = false;			
	}
	else if (btn_unlock == true){
		set_servo(SERVO_UNLOCK, SOURCE_BUTTONS);
		btn_unlock = false;			
	}

	// Lock disable timeout	
	if (servo_status > SERVO_DISABLE) {
		if (current_timestamp >= servo_timestamp) {
			if (servo_status < SERVO_CENTER) {
				set_servo(SERVO_CENTER, source_status);
			}else{	
				set_servo(SERVO_DISABLE, source_status);
			}
		}
	}


	// LED display
	if (servo_status == SERVO_LOCK){
		if (current_timestamp >= activity_timestamp) {
			if (activity_flash == 0){
				digitalWrite(LED_LOCK_PIN, HIGH);
				digitalWrite(LED_UNLOCK_PIN, LOW);
				activity_flash = 1;
			}
			else{
				digitalWrite(LED_LOCK_PIN, LOW);
				digitalWrite(LED_UNLOCK_PIN, LOW);
				activity_flash = 0;
			}
			activity_timestamp = current_timestamp + 50;
		}
	}
	else if (servo_status == SERVO_UNLOCK){
		if (current_timestamp >= activity_timestamp) {
			if (activity_flash == 0){
				digitalWrite(LED_LOCK_PIN, LOW);
				digitalWrite(LED_UNLOCK_PIN, HIGH);
				activity_flash = 1;
			}
			else{
				digitalWrite(LED_LOCK_PIN, LOW);
				digitalWrite(LED_UNLOCK_PIN, LOW);
				activity_flash = 0;
			}
			activity_timestamp = current_timestamp + 50;
		}
	}
	else if (servo_status == SERVO_DISABLE){
		if (sensor_close == false || sensor_handle == false){
			if (current_timestamp >= activity_timestamp) {
				if (activity_flash == 0){
					digitalWrite(LED_LOCK_PIN, LOW);
					digitalWrite(LED_UNLOCK_PIN, HIGH);
					activity_flash = 1;
				}
				else{
					digitalWrite(LED_LOCK_PIN, LOW);
					digitalWrite(LED_UNLOCK_PIN, LOW);
					activity_flash = 0;
				}
				activity_timestamp = current_timestamp + 500;
			}
		}
		else if (sensor_disable == true){
			if (current_timestamp >= activity_timestamp) {
				if (activity_flash == 0){
					digitalWrite(LED_LOCK_PIN, HIGH);
					digitalWrite(LED_UNLOCK_PIN, LOW);
					activity_flash = 1;
				}
				else{
					digitalWrite(LED_LOCK_PIN, LOW);
					digitalWrite(LED_UNLOCK_PIN, LOW);
					activity_flash = 0;
				}
				activity_timestamp = current_timestamp + 500;
			}
		}
		else if (sensor_lock == true){
			digitalWrite(LED_LOCK_PIN, HIGH);
			digitalWrite(LED_UNLOCK_PIN, LOW);
		}
		else{
			digitalWrite(LED_LOCK_PIN, LOW);
			digitalWrite(LED_UNLOCK_PIN, HIGH);
		}
	}
}

void setter(byte value) {
}

byte getter(void) {
  // return previously saved (in getter()) value
  return currentLEDValue;
}