#include "ArduinoSTL.h";
#include "tone.h";

// Miscallaneous inputs/outputs
#define UNARMED_LED 2       // LED to light up when unarmed
#define ARMED_LED 13        // LED to light up when armed
#define ARM_UNARM_BUTTON 3  // button used to arm/unarm
#define SPKR 12

using namespace std;

// Configuration
const String pass = "113";  // password

const int maxWarnLevel  = 250;  // fastest beeping speed of alarm in ms
const int warnDelayRef  = 300;  // initial beeping speed of alarm
const int warnIncrement = 7;    // speed of alarm beeping acceleration

// Program variables
bool isArmed         = 1;  // set to initialize as armed or unarmed
bool soundAlarm      = 0;
bool unarmButtonDown = 0;  // unarm after arm only until button is released
bool waitingForPass  = 0;

int warnLevel = 0;  // warn level of alarm

String readString;  // temporary string to store serial input on pass entry

unsigned long currentMillis   = millis();  // reference point for timed stuff
unsigned long unarmIntTracker = 0;    // ref point for unarm timing interval
unsigned long alarmIntTracker = 0;    // ref point for alarm timing interval
bool flipflopUnarmState       = LOW;  // unarm led blinking status
bool flipflopAlarmState       = LOW;  // alarm led and beep blink status

// Sounds
int initMelody[]    = {FS4, GS4, AS4, CS5, DS5, FS5};
int initDurations[] = {100, 100, 100, 100, 100, 100};

int unarmMelody[]    = {B3, B2};
int unarmDurations[] = {50, 50};

int armMelody[]    = {C5, C6};
int armDurations[] = {50, 50};

int alarmNote = CS5;

int noteDelay = 40;

// Security Point (SP) storage
vector<String> SPnames;  // names
vector<int> SPpins;      // pins that a Security Point is connected
vector<int> SPnormpos;   // normal position of sensor, 0=closed 1=open
vector<bool> SPisopen;   // whether the point is open or closed

void setup() {
	Serial.begin(9600);

	// Add security points here
	addSecurityPoint("Door1", 4, 1);
	addSecurityPoint("Door2", 5, 1);

	// Misc. inputs/outputs
	pinMode(ARM_UNARM_BUTTON, INPUT_PULLUP);
	pinMode(UNARMED_LED, OUTPUT);
	pinMode(ARMED_LED, OUTPUT);

	Serial.println("Initialized.");

	playMelody(6, initMelody, initDurations, noteDelay);

	if (isArmed == 1) {
		Serial.println("Currently ARMED.");
		digitalWrite(ARMED_LED, HIGH);
	} else {
		Serial.println("Currently UNARMED.");
		digitalWrite(UNARMED_LED, HIGH);
	}
}

// Adds a Security Point by adding to each of the vectors.
void addSecurityPoint(String name, int pin, bool defaultPosition) {
	SPnames.push_back(name);
	SPpins.push_back(pin);
	SPnormpos.push_back(defaultPosition);
	SPisopen.push_back(false);

	pinMode(pin, INPUT_PULLUP);
	Serial.println(name + " has been added.");
}

// Play an array of tones.
void playMelody(int melodyLength, int *melody, int *noteDur, int noteDelay) {
	for (int i = 0; i < melodyLength; i++) {
		tone(SPKR, melody[i], noteDur[i]);
		delay(noteDelay);
		noTone(SPKR);
	}
}

// Switches an LED's power on/off whenever called.
void flipLed(int ledPin, bool &state) {
	if (state == LOW) {
		state = HIGH;
	} else {
		state = LOW;
	};

	digitalWrite(ledPin, state);
}

void loop() {
	// - Iterates through all security points -

	for (int i = 0; i <= SPnames.size(); i++) {
		if (digitalRead(SPpins[i]) != SPnormpos[i]) {

			if (SPisopen[i] == false) {
				SPisopen[i] = true;
				Serial.println(SPnames[i] + " has opened.");
			}

			if (isArmed == true && soundAlarm == 0) soundAlarm = 1;

		} else if (digitalRead(SPpins[i]) == SPnormpos[i]) {
			
			if (SPisopen[i] == true) {
				SPisopen[i] = false;
				Serial.println(SPnames[i] + " has closed.");
			}

			if (soundAlarm == 1) soundAlarm = 0;
		}
	}

	// - Arm/Unarm -

	if ((digitalRead(ARM_UNARM_BUTTON) == LOW || waitingForPass == 1) &&
	    unarmButtonDown == 0) {
		if (isArmed == 1 || waitingForPass == 1) {
			if (waitingForPass == 0) {
				waitingForPass = 1;
				Serial.println("To unarm, please enter the password.");
				Serial.print("password> ");
			}

			if (Serial.available()) {
				char c = Serial.read();

				if (c != '\n') {
					readString += c;
				} else {
					Serial.println(readString);

					if (readString == pass) {
						isArmed = 0;

						playMelody(2, unarmMelody, unarmDurations, noteDelay);
						Serial.println("Successfully UNARMED.");
					} else {
						playMelody(2, armMelody, unarmDurations, noteDelay);
						Serial.println("Wrong password!");
					}
					readString     = "";
					waitingForPass = 0;
				}
			}

			currentMillis = millis();

			// async blink unarm led every 250ms
			if (currentMillis - unarmIntTracker >= 1000 && waitingForPass == 1) {
				unarmIntTracker = currentMillis;
				flipLed(UNARMED_LED, flipflopUnarmState);
			}
		} else {
			isArmed         = 1;
			waitingForPass  = 0;
			unarmButtonDown = 1;

			playMelody(2, armMelody, armDurations, noteDelay);
			Serial.println("Successfully ARMED.");
		}
	}

	// - Button safety -

	if (digitalRead(ARM_UNARM_BUTTON) == HIGH && unarmButtonDown == 1) {
		unarmButtonDown = 0;
	}

	// - Ignore Serial input unless we're waiting for it -

	if (waitingForPass == 0) {
		while (Serial.available()) { Serial.read(); }
	}

	// - Activate unarmed/armed LEDs -

	if (isArmed == 1 && waitingForPass == 0 && soundAlarm == 0) {
		digitalWrite(ARMED_LED, HIGH);
		digitalWrite(UNARMED_LED, LOW);
	} else if (waitingForPass == 0 && soundAlarm == 0) {
		digitalWrite(UNARMED_LED, HIGH);
		digitalWrite(ARMED_LED, LOW);
	}

	// - Sound alarm -

	if (soundAlarm == 1) {
		currentMillis = millis();

		if ((currentMillis - alarmIntTracker) >= (warnDelayRef - warnLevel)) {
			alarmIntTracker = currentMillis;

			if (warnLevel < maxWarnLevel) { warnLevel = warnLevel + warnIncrement; };

			flipLed(ARMED_LED, flipflopAlarmState);

			if (flipflopAlarmState == HIGH) {
				tone(SPKR, alarmNote);
			} else {
				noTone(SPKR);
			}
		}
	} else {
		warnLevel = 0;
		noTone(SPKR);
	}
}