#include <ZUNO_SERVO.h>
#include <EEPROM.h>

#define CONSOLE_SERIAL Serial
#define RFID_SERIAL Serial0

#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 BUZZER_PIN = 11;

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

#define SERVO_TRIM 2

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

#define SIGNAL_DEBOUNCE_CONSTANT 30

bool sensor_door_locked = false;
bool sensor_handle_in_up_position = false;
bool sensor_not_home_activated = false;
bool sensor_door_closed = false;

bool sensor_door_locked_changed = false;
bool sensor_door_closed_changed = false;
bool sensor_not_home_activated_changed = false;

int sensor_door_locked_prev_state = -1;
int sensor_door_locked_current_state = LOW;
int sensor_door_locked_debounce = 0;
int sensor_handle_in_up_position_prev_state = -1;
int sensor_handle_in_up_position_current_state = LOW;
int sensor_handle_in_up_position_debounce = 0;
int sensor_not_home_activated_prev_state = -1;
int sensor_not_home_activated_current_state = LOW;
int sensor_not_home_activated_debounce = 0;
int sensor_door_closed_prev_state = -1;
int sensor_door_closed_current_state = LOW;
int sensor_door_closed_debounce = 0;

bool btn_lock = false;
bool btn_unlock = false;

int btn_lock_prev_state = -1;
int btn_lock_current_state = LOW;
int btn_lock_debounce = 0;
int btn_unlock_prev_state = -1;
int btn_unlock_current_state = LOW;
int btn_unlock_debounce = 0;

int activity_flash = 0;
int buzzer_flash = 0;
int servo_status = 0;
int source_status = 0;

unsigned long current_timestamp = 0;
unsigned long activity_timestamp = 0;
unsigned long servo_timestamp = 0;
unsigned long rfid_timeout_timestamp = 0;
unsigned long alarm_timestamp = 0;
unsigned long buzzer_timestamp = 0;

bool lock_disable = false;

#define RFID_PACKAGE_LENGHT 14 // nmbr of bytes in package
#define RFID_START_BYTE 0x02   // start byte
#define RFID_STOP_BYTE 0x03    // stop byte
#define RFID_TAG_BYTES 10 // number of bytes in package that represent the tag
#define RFID_TAGS 10       // Number of tags to be supported to store in EEPROM
#define EEPROM_TAG_START_ADDRESS 0 // Start addres for storing RFID tags
int rfid_current_checksum = 0;
int rfid_package[RFID_PACKAGE_LENGHT];
int rfid_package_pos = 0;
int rfid_tags[RFID_TAGS][RFID_TAG_BYTES];
int rfid_store_tag_nbr = 0;
bool rfid_store_tag = false;

#define ALARM_PRE_STAGE_DISABLED 0
#define ALARM_PRE_STAGE_WAITING 1
#define ALARM_PRE_STAGE_ENABLED 2

int alarm_pre_activate = ALARM_PRE_STAGE_DISABLED;
bool alarm_activate = false;
bool alarm_confirmed = false;
bool alarm_waiting_for_confirmation = false;

ServoController servo(12);

ZUNO_SETUP_SLEEPING_MODE(ZUNO_SLEEPING_MODE_ALWAYS_AWAKE);

// set up channel
ZUNO_SETUP_CHANNELS(
	ZUNO_SWITCH_BINARY(getterLock, setterLock),
	ZUNO_SWITCH_BINARY(getterLockDisable, setterLockDisable),
	ZUNO_SWITCH_BINARY(getterAlarmActivate, setterAlarmActivate),
	ZUNO_SWITCH_BINARY(getterAlarmConfirm, setterAlarmConfirm),
	ZUNO_SWITCH_MULTILEVEL(getterStoreRFID, setterStoreRFID),
	ZUNO_SENSOR_BINARY_DOOR_WINDOW(getterDoor)
);

void set_servo(int mode, int source){
	bool ok_to_lock_unlock = false;

	// Check sensor status if ok to lock/unlock	
	if (sensor_handle_in_up_position == true && sensor_not_home_activated == false && sensor_door_closed == true && lock_disable == false){
		ok_to_lock_unlock = true;
	}

	// Set servo position
	if (mode == SERVO_LOCK && ok_to_lock_unlock && sensor_door_locked == false){
		servo.setValue(160+SERVO_TRIM);
		servo_status = mode;
		source_status = source;
		servo.begin();
		digitalWrite(SERVO_ENABLE_PIN, HIGH);
		servo_timestamp = current_timestamp + 700;
	}
	else if (mode == SERVO_UNLOCK && ok_to_lock_unlock && sensor_door_locked == true){
		servo.setValue(20+SERVO_TRIM);
		servo_status = mode;
		source_status = source;
		servo.begin();
		digitalWrite(SERVO_ENABLE_PIN, HIGH);
		servo_timestamp = current_timestamp + 700;
	}
	else if (mode == SERVO_CENTER){
		servo.setValue(90+SERVO_TRIM);
		servo_status = mode;
		servo.begin();
		digitalWrite(SERVO_ENABLE_PIN, HIGH);
		servo_timestamp = current_timestamp + 700;
	}
	else if (mode == SERVO_DISABLE){
		digitalWrite(SERVO_ENABLE_PIN, LOW);
		servo_status = mode;
	}
}

void load_rfid_keys() {
	int address = EEPROM_TAG_START_ADDRESS;
	for (int i = 0; i < RFID_TAGS; i++) {
		for (int j = 0; j < RFID_TAG_BYTES; j++) {
			rfid_tags[i][j] = EEPROM.read(address);
			address++;
		}
	}
	CONSOLE_SERIAL.println("RFID tags loaded from EEPROM");
}

void update_rfid() {
	int rfid_read = 0;
	if (RFID_SERIAL.available()) {
		while (RFID_SERIAL.available() > 0) {
			rfid_read = RFID_SERIAL.read();

			// Check for package start
			if (rfid_read == RFID_START_BYTE) {
				rfid_package_pos = 0;
			}
			// Check for package end
			if (rfid_read == RFID_STOP_BYTE) {
				CONSOLE_SERIAL.println("RFID tag detected");
				if (rfid_store_tag) {
					int rfid_eeprom_pos = (rfid_store_tag_nbr-1) * RFID_TAG_BYTES;
					for (int i = 0; i < RFID_TAG_BYTES; i++) {
						EEPROM.write(rfid_eeprom_pos, rfid_package[i + 1]);
						rfid_tags[rfid_store_tag_nbr-1][i] = rfid_package[i + 1];
						rfid_eeprom_pos++;
					}

					CONSOLE_SERIAL.println("RFID tag stored");
					rfid_store_tag = false;	
					rfid_store_tag_nbr = 0;	
					zunoSendReport(5);
					
				} else {
					// Check for match of RFID tag
					for (int i = 0; i < RFID_TAGS; i++) {
						bool tag_ok = true;
						for (int j = 0; j < RFID_TAG_BYTES; j++) {
							if (rfid_package[j + 1] != rfid_tags[i][j]) {
								tag_ok = false;
							}
						}
						if (tag_ok == true) {
							CONSOLE_SERIAL.println("RFID tag verified");
							if (sensor_door_locked){
								set_servo(SERVO_UNLOCK, SOURCE_RFID);
								if (alarm_activate == true){
									alarm_activate = false;
									alarm_waiting_for_confirmation = true;
									zunoSendReport(3);
								}
							}
							else{
								set_servo(SERVO_LOCK, SOURCE_RFID);
								if (alarm_pre_activate == ALARM_PRE_STAGE_ENABLED){
									alarm_pre_activate = ALARM_PRE_STAGE_DISABLED;
									alarm_activate = true;
									alarm_waiting_for_confirmation = true;
									zunoSendReport(3);
								}
							}
							break;
						}
					}
				}
			} else {
				rfid_package[rfid_package_pos] = rfid_read;
			}
			if (rfid_package_pos < RFID_PACKAGE_LENGHT - 1) {
				rfid_package_pos++;
			}
		}
	}
}

void update_buttons(){
	sensor_door_locked_current_state = digitalRead(SENSOR_LOCK_PIN);
	sensor_handle_in_up_position_current_state = digitalRead(SENSOR_HANDLE_PIN);
	sensor_not_home_activated_current_state = digitalRead(SENSOR_DISABLE_PIN);
	sensor_door_closed_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_door_locked_current_state != sensor_door_locked_prev_state) {
		if (++sensor_door_locked_debounce == SIGNAL_DEBOUNCE_CONSTANT) {
			if (sensor_door_locked_current_state == LOW) {
				sensor_door_locked = false;
			}
			else{
				sensor_door_locked = true;
			}
			sensor_door_locked_prev_state = sensor_door_locked_current_state;
			sensor_door_locked_debounce = 0;
			sensor_door_locked_changed = true;
		}
	} else {
		sensor_door_locked_debounce = 0;
	}
	if (sensor_handle_in_up_position_current_state != sensor_handle_in_up_position_prev_state) {
		if (++sensor_handle_in_up_position_debounce == SIGNAL_DEBOUNCE_CONSTANT) {
			if (sensor_handle_in_up_position_current_state == LOW) {
				sensor_handle_in_up_position = true;
			}
			else{
				sensor_handle_in_up_position = false;
			}
			sensor_handle_in_up_position_prev_state = sensor_handle_in_up_position_current_state;
			sensor_handle_in_up_position_debounce = 0;
		}
	} else {
		sensor_handle_in_up_position_debounce = 0;
	}
	if (sensor_not_home_activated_current_state != sensor_not_home_activated_prev_state) {
		if (++sensor_not_home_activated_debounce == SIGNAL_DEBOUNCE_CONSTANT) {
			if (sensor_not_home_activated_current_state == LOW) {
				sensor_not_home_activated = false;
			}
			else{
				sensor_not_home_activated = true;
			}
			sensor_not_home_activated_prev_state = sensor_not_home_activated_current_state;
			sensor_not_home_activated_debounce = 0;
			sensor_not_home_activated_changed = true;
		}
	} else {
		sensor_not_home_activated_debounce = 0;
	}
	if (sensor_door_closed_current_state != sensor_door_closed_prev_state) {
		if (++sensor_door_closed_debounce == SIGNAL_DEBOUNCE_CONSTANT) {
			if (sensor_door_closed_current_state == LOW) {
				sensor_door_closed = true;
			}
			else{
				sensor_door_closed = false;
			}
			sensor_door_closed_prev_state = sensor_door_closed_current_state;
			sensor_door_closed_debounce = 0;
			sensor_door_closed_changed = true;
		}
	} else {
		sensor_door_closed_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() {

	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);

	CONSOLE_SERIAL.begin(9600);
	RFID_SERIAL.begin(9600);
	
	load_rfid_keys();
	set_servo(SERVO_CENTER, SOURCE_SYSTEM);	
  	
	pinMode(BUZZER_PIN, OUTPUT); // setup pin as output
	digitalWrite(BUZZER_PIN, LOW);

	// Test (show) indicators and buzzer
	digitalWrite(BUZZER_PIN, HIGH);
	digitalWrite(LED_UNLOCK_PIN, HIGH);
	digitalWrite(LED_LOCK_PIN, HIGH);
	delay(500);
	digitalWrite(BUZZER_PIN, LOW);
	digitalWrite(LED_UNLOCK_PIN, LOW);
	digitalWrite(LED_LOCK_PIN, LOW);
	delay(1000);
		
}

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

	// Update all buttons and sensors	
	update_buttons();
	update_rfid();

	// Check button status
	if (btn_lock == true){
		if (rfid_store_tag_nbr > 0 && rfid_store_tag == false){
			int rfid_eeprom_pos = (rfid_store_tag_nbr-1) * RFID_TAG_BYTES;
			for (int i = 0; i < RFID_TAG_BYTES; i++) {
				EEPROM.write(rfid_eeprom_pos,0xFF);
				rfid_eeprom_pos++;
			}
			rfid_store_tag_nbr = 0;
			zunoSendReport(5);
		}
		else{
			set_servo(SERVO_LOCK, SOURCE_BUTTONS);
		}
		btn_lock = false;			
		btn_unlock = false;			
	}
	else if (btn_unlock == true){
		if (rfid_store_tag_nbr > 0 && rfid_store_tag == false){
			rfid_store_tag = true;
		}
		else{
			set_servo(SERVO_UNLOCK, SOURCE_BUTTONS);
			if (sensor_door_locked == false){
				alarm_pre_activate = ALARM_PRE_STAGE_WAITING;
				alarm_timestamp = current_timestamp + 2000;
			}
		}
		btn_lock = false;			
		btn_unlock = false;			
	}

	// Check if sensor changed and report to Z-Wave
	if (sensor_door_locked_changed){
		sensor_door_locked_changed = false;
		zunoSendReport(1);
	}

	// Check if sensor changed and report to Z-Wave
	if (sensor_not_home_activated_changed){
		sensor_not_home_activated_changed = false;
		zunoSendReport(2);
	}

	// Check if sensor changed and report to Z-Wave
	if (sensor_door_closed_changed){
		sensor_door_closed_changed = false;
		zunoSendReport(6);
	}

	// Check if servo needs to be centered or turned off
	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);
			}
		}
	}

	// Check if pre Alarm should be enablede
	if (alarm_pre_activate == ALARM_PRE_STAGE_WAITING) {
		if (current_timestamp >= alarm_timestamp) {
			if (sensor_door_locked_current_state == LOW){ 
				alarm_pre_activate = ALARM_PRE_STAGE_ENABLED;
			}
			else {
				alarm_pre_activate = ALARM_PRE_STAGE_DISABLED;
			}
		}
	}

	// Check if store RFID has timed out
	if (rfid_store_tag_nbr > 0) {
		if (current_timestamp >= rfid_timeout_timestamp) {
			rfid_store_tag = false;
			rfid_store_tag_nbr = 0;	
			zunoSendReport(6);
		}
	}
	
	// Buzzer indication for ALARM status
	if (alarm_waiting_for_confirmation == true){
		digitalWrite(BUZZER_PIN, HIGH);
	}
	else if (alarm_pre_activate == ALARM_PRE_STAGE_ENABLED){
		if (current_timestamp >= buzzer_timestamp) {
			if (buzzer_flash == 0){
				digitalWrite(BUZZER_PIN, HIGH);
				buzzer_flash = 1;
				buzzer_timestamp = current_timestamp + 100;
			}
			else{
				digitalWrite(BUZZER_PIN, LOW);
				buzzer_flash = 0;
				buzzer_timestamp = current_timestamp + 1000;
			}
		}
	}
	else{
		digitalWrite(BUZZER_PIN, LOW);
	}

	// Indicate that the door is being locked
	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;
		}
	}

	// Indicate that the door is being unlocked
	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){
		// Indicate RFID storing mode
		if (rfid_store_tag_nbr > 0){
			digitalWrite(LED_LOCK_PIN, HIGH);
			digitalWrite(LED_UNLOCK_PIN, HIGH);
		}
		// Indicate that pre Alarm activation has been enabled
		else if (alarm_pre_activate == ALARM_PRE_STAGE_ENABLED){
			if (current_timestamp >= activity_timestamp) {
				if (activity_flash == 0){
					digitalWrite(LED_LOCK_PIN, HIGH);
					digitalWrite(LED_UNLOCK_PIN, HIGH);
					activity_flash = 1;
					activity_timestamp = current_timestamp + 100;
				}
				else{
					digitalWrite(LED_LOCK_PIN, LOW);
					digitalWrite(LED_UNLOCK_PIN, HIGH);
					activity_flash = 0;
					activity_timestamp = current_timestamp + 1000;
				}
			}
		}
		// Indicate that the lock/unlock is disabled
		else if (sensor_not_home_activated == true || lock_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;
			}
		}
		// Indicate that the door is open
		else if (sensor_door_closed == false || sensor_handle_in_up_position == 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;
			}
		}
		// Indicate that the door is locked
		else if (sensor_door_locked == true){
			digitalWrite(LED_LOCK_PIN, HIGH);
			digitalWrite(LED_UNLOCK_PIN, LOW);
		}
		// Indicate that the door is unlocked
		else{
			digitalWrite(LED_LOCK_PIN, LOW);
			digitalWrite(LED_UNLOCK_PIN, HIGH);
		}
	}
}

void setterLock(byte value) {
	if (value == 0){
		set_servo(SERVO_UNLOCK, SOURCE_ZWAVE);
	}
	else{
		set_servo(SERVO_LOCK, SOURCE_ZWAVE);
	}
}

byte getterLock(void) {
	return sensor_door_locked;
}

void setterLockDisable(byte value) {
	if (value == 0){
		lock_disable = false;
	}
	else{
		lock_disable = true;
	}
}

byte getterLockDisable(void) {
	return lock_disable;
}

void setterAlarmActivate(byte value) {
	if (value == 0){
		alarm_activate = false;
	}
	else{
		alarm_activate = true;
	}
}

byte getterAlarmActivate(void) {
	return alarm_activate;
}

void setterAlarmConfirm(byte value) {
	if (value == 0){
		alarm_confirmed = false;
		if (alarm_activate == false){
			alarm_waiting_for_confirmation == false;
		}
	}
	else{
		alarm_confirmed = true;
		if (alarm_activate == true){
			alarm_waiting_for_confirmation == false;
		}
	}
}

byte getterAlarmConfirm(void) {
	return alarm_confirmed;
}

void setterStoreRFID(byte value) {
	if (rfid_store_tag_nbr == 0 && value > 0 && value < RFID_TAGS + 1){
		rfid_store_tag_nbr = value;
		rfid_timeout_timestamp = current_timestamp + 30000;
	}
	else if (value == 0){
		rfid_store_tag_nbr = 0;
		rfid_store_tag = false;
	}
}

byte getterStoreRFID(void) {
	return rfid_store_tag_nbr;
}

byte getterDoor(void) {
	return sensor_door_closed;
}