#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <Adafruit_FeatherOLED.h>

#include "hardware.h"

#define SIMULATE 1
#define DEBUG 0

#define TIMEOUT_TIME 7000
#define BUTTON_HOLD_TIME 500
#define BUTTON_PRESS_TIME 20

#define UP_BUTTON 31
#define ENTER_BUTTON 30
#define DOWN_BUTTON 27

Adafruit_FeatherOLED oled = Adafruit_FeatherOLED();

//static const unsigned char PROGMEM arrow[] =
//{ B10000000,
//	B11000000,
//	B11100000,
//	B11000000,
//	B10000000};
//oled.drawBitmap(0, 1, arrow, 8, 5, 1);


enum buttonStates {
	OPEN,
	CLOSED,
	CHECK_PRESSED,
	PRESSED,
	HELD,
	NUM_BUTTONSTATES
};

enum screenStates {
	BOOT_UP,
	PRESSURE,
	SET_POINT,
	INIT_RUN,
	BEGIN_RUN,
	MEASURING,
	RUNNING,
	SAY_DONE,
	SAY_CANCEL,
	SAY_HOLD,
	SAY_TIMEOUT,
	SETTINGS,
};

static const char* stateLabels[] = {
	"BOOT_UP",
	"PRESSURE",
	"SET_POINT",
	"INIT_RUN",
	"BEGIN_RUN",
	"MEASURING",
	"RUNNING",
	"SAY_DONE",
	"SAY_CANCEL",
	"SAY_HOLD",
	"SAY_TIMEOUT",
	"SETTINGS",
};


enum buttonStates upButton = OPEN;
enum buttonStates enterButton = OPEN;
enum buttonStates downButton = OPEN;

enum screenStates screenState = BOOT_UP;
double pressureValue = 0.0;
int pressureSetPoint = 0;
double initialPressure = 0.0;
//double runningPressure = 0.0;
double sampledPressure = 0.0;
double runningRateInv = 0.0;

int debounceValue(double value) {
	static int prevValue = (int) value;

	if (abs(prevValue - value) > 0.4) {
		prevValue = (int) value;
	}

	return prevValue;
}

void setup()
{
	randomSeed(analogRead(A1));

	Serial.begin(115200);

	oled.init();

	pinMode(UP_BUTTON, INPUT_PULLUP);
	pinMode(ENTER_BUTTON, INPUT); // Has external pullup
	pinMode(DOWN_BUTTON, INPUT_PULLUP);

	pinMode(RELAY1_PIN, OUTPUT);
	pinMode(RELAY2_PIN, OUTPUT);
	digitalWrite(RELAY1_PIN, HIGH);
	digitalWrite(RELAY2_PIN, HIGH);

#ifdef SIMULATE
	initSimulation();
#endif

	resamplePressure(pressureValue);
}

void loop() {
	measurePressure(pressureValue);
	pollButtons();
	runUI();
	logData();

#ifdef SIMULATE
	tickSimulation();
#endif
}

void logData() {
	static unsigned long lastLogTime = millis();

	if (millis() > lastLogTime + 100) {
		Serial.print("pressure: ");
		Serial.print(pressureValue);
#ifdef SIMULATE
		Serial.print(", simulated: ");
		Serial.print(simulatedPressure);
#endif
		Serial.print(", sampled: ");
		Serial.print(sampledPressure);
		Serial.print(", rate (inv): ");
		Serial.print(runningRateInv);
		Serial.print(", setpoint: ");
		Serial.print(pressureSetPoint);
		Serial.print(", state: ");
		Serial.print(stateLabels[screenState]);
		Serial.println("");

		lastLogTime = millis();
	}
}

void runUI() {
	static enum screenStates nextState = BOOT_UP;

	static unsigned long timer = millis();

	static unsigned long startTime = millis();
	static unsigned long stopTime = millis();
	static bool isInflating = false;
	static bool isDeflating = false;

	int num_dots = 0;
	int debouncedPressureValue = debounceValue(pressureValue);

	oled.clearDisplay();

	switch (screenState) {
		case BOOT_UP:
			if (millis() >= timer + 2000) {
				screenState = PRESSURE;
			}

			oled.setCursor(0,0);
			oled.setTextSize(2);
			oled.println("STC TECH");

			oled.setTextSize(1);
			oled.println("");
			oled.print("     INITIALIZING");

			oled.display();
			break;

		case PRESSURE:
			if (enterButton == PRESSED) {
				screenState = SAY_HOLD;
				nextState = PRESSURE;
				timer = millis();
			} else if (enterButton == HELD) {
				;  // settings?
			} else if (upButton == PRESSED) {
				screenState = SET_POINT;
				pressureSetPoint = debouncedPressureValue+1;
				timer = millis();
			} else if (downButton == PRESSED) {
				screenState = SET_POINT;
				pressureSetPoint = debouncedPressureValue-1;
				timer = millis();
			} else if (upButton == HELD) {
				screenState = SET_POINT;
				pressureSetPoint = debouncedPressureValue+1;
				timer = millis();
			} else if (downButton == HELD) {
				screenState = SET_POINT;
				pressureSetPoint = debouncedPressureValue-1;
				timer = millis();
			}

			oled.setCursor(0,0);
			oled.setTextSize(3);
			oled.print(debouncedPressureValue);
			oled.print(" PSI");

			oled.display();
			break;

		case SET_POINT:
			if (enterButton == PRESSED) {
				screenState = SAY_HOLD;
				nextState = SET_POINT;
				timer = millis();
			} else if (enterButton == HELD) {
				timer = millis();
				screenState = INIT_RUN;
			} else if (upButton == PRESSED) {
				timer = millis();
				pressureSetPoint++;
			} else if (downButton == PRESSED) {
				timer = millis();
				pressureSetPoint--;
			} else if (upButton == HELD) {
				timer = millis();
				pressureSetPoint++;
				delay(75);
			} else if (downButton == HELD) {
				timer = millis();
				pressureSetPoint--;
				delay(75);
			} else if (millis() >= timer + TIMEOUT_TIME) {
				screenState = SAY_TIMEOUT;
				nextState = PRESSURE;
				timer = millis();
			}

			oled.setCursor(0,0);
			oled.setTextSize(3);
			oled.print(pressureSetPoint);
			oled.println(" PSI");
			oled.setTextSize(1);
			oled.println("TARGET");

			oled.display();

			break;

		case INIT_RUN:
			initialPressure = pressureValue;
			startTime = millis();
			timer = millis();
			screenState = BEGIN_RUN;

			oled.setCursor(0,0);
			oled.setTextSize(1);
			oled.println("");
			oled.setTextSize(2);
			if (pressureSetPoint > debouncedPressureValue) {
				oled.print("INFLATING");
				setSoleniod(SOLENOID_INFLATE);
				isInflating = true;
				isDeflating = false;
			} else if (pressureSetPoint < debouncedPressureValue) {
				oled.print("DEFLATING");
				setSoleniod(SOLENOID_DEFLATE);
				isDeflating = true;
				isInflating = false;
			} else {
				setSoleniod(SOLENOID_STOP);
				screenState = SAY_DONE;
				nextState = PRESSURE;
			}

			oled.display();

			break;

		case BEGIN_RUN:
			if (millis() >= timer + 5000) {
				//runningPressure = pressureValue;
				setSoleniod(SOLENOID_STOP);
				screenState = MEASURING;
				timer = millis();
				stopTime = millis();
			}

			break;

		case MEASURING:
			if (millis() >= timer + 3000) {
				sampledPressure = pressureValue;

				runningRateInv = (stopTime - startTime) / (sampledPressure - initialPressure);

				Serial.print("stopTime: ");
				Serial.print(stopTime);
				Serial.print(", startTime: ");
				Serial.print(startTime);
				Serial.print(", sampledPressure: ");
				Serial.print(sampledPressure);
				Serial.print(", initialPressure: ");
				Serial.print(initialPressure);
				Serial.print(", rate: ");
				Serial.print(runningRateInv);
				Serial.println("");

				initialPressure = sampledPressure;

				if (isInflating && (int) sampledPressure >= pressureSetPoint) {
					screenState = SAY_DONE;
					nextState = PRESSURE;
				} else if (isDeflating && (int) sampledPressure <= pressureSetPoint) {
					screenState = SAY_DONE;
					nextState = PRESSURE;
				} else {
					screenState = RUNNING;
				}

				timer = millis();
				startTime = millis();
			}

			if (millis() < timer + 500) {
				// wait for solenoids to settle before averaging
				resamplePressure(pressureValue);
			}

			setSoleniod(SOLENOID_STOP);

			oled.setCursor(0,0);
			oled.setTextSize(1);
			oled.println("");
			oled.setTextSize(2);
			oled.print("MEASURING");
			oled.display();

			break;

		case RUNNING:
			if (enterButton == PRESSED) {
				setSoleniod(SOLENOID_STOP);
				screenState = SAY_CANCEL;
				nextState = PRESSURE;
				timer = millis();
			} else if (upButton == PRESSED) {
				setSoleniod(SOLENOID_STOP);
				screenState = SAY_CANCEL;
				nextState = PRESSURE;
				timer = millis();
			} else if (downButton == PRESSED) {
				setSoleniod(SOLENOID_STOP);
				screenState = SAY_CANCEL;
				nextState = PRESSURE;
				timer = millis();
			}

			if (isInflating && millis() >= timer + 20000) {
				screenState = MEASURING;
				timer = millis();
				stopTime = millis();
			} else if (isDeflating && millis() >= timer + 10000) {
				screenState = MEASURING;
				timer = millis();
				stopTime = millis();
			}

			if (isInflating) {
				setSoleniod(SOLENOID_INFLATE);
			} else if (isDeflating) {
				setSoleniod(SOLENOID_DEFLATE);
			}

			oled.setCursor(0,0);
			oled.setTextSize(3);
			oled.print((int) sampledPressure);
			oled.println(" PSI");

			oled.setTextSize(1);
			if (isInflating) {
				oled.print("INFLATING");
			} else if (isDeflating) {
				oled.print("DEFLATING");
			}
			num_dots = (int) ((millis() - startTime) / 400) % 4;
			for (int i = 0; i < num_dots; i++) {
				oled.print(".");
			}

			oled.display();

			break;



		case SAY_DONE:
			if (millis() >= timer + 3000) {
				screenState = nextState;
				timer = millis();
			}

			oled.setCursor(0,0);
			oled.setTextSize(3);
			oled.print("DONE");

			oled.display();

			break;

		case SAY_CANCEL:
			if (millis() >= timer + 1000) {
				screenState = nextState;
				timer = millis();
			}

			oled.setCursor(0,0);
			oled.setTextSize(3);
			oled.print("CANCEL");

			oled.display();

			break;

		case SAY_HOLD:
			if (millis() >= timer + 500) {
				screenState = nextState;
				timer = millis();
			}

			oled.setCursor(0,0);
			oled.setTextSize(3);
			oled.print("HOLD");

			oled.display();

			break;

		case SAY_TIMEOUT:
			if (millis() >= timer + 1000) {
				screenState = nextState;
				timer = millis();
			}

			oled.setCursor(0,0);
			oled.setTextSize(3);
			oled.print("TIMEOUT");

			oled.display();

			break;
	}
}

void pollButtons() {
	static unsigned long upButtonTime = 0;
	static unsigned long enterButtonTime = 0;
	static unsigned long downButtonTime = 0;

	processButtonState(UP_BUTTON, upButton, upButtonTime);
	processButtonState(ENTER_BUTTON, enterButton, enterButtonTime);
	processButtonState(DOWN_BUTTON, downButton, downButtonTime);

	if (DEBUG) {
		if (upButton == PRESSED) {
			Serial.println("up button pressed");
		} else if (upButton == HELD) {
			Serial.println("up button held");
		}

		if (enterButton == PRESSED) {
			Serial.println("enter button pressed");
		} else if (enterButton == HELD) {
			Serial.println("enter button held");
		}

		if (downButton == PRESSED) {
			Serial.println("down button pressed");
		} else if (downButton == HELD) {
			Serial.println("down button held");
		}
	}
}

void processButtonState(int buttonPin, buttonStates &buttonState, unsigned long &buttonTime) {
	bool pinState = !digitalRead(buttonPin);

	switch(buttonState) {
		case OPEN:
			if (pinState) {
				buttonState = CLOSED;
				buttonTime = millis();
			}
			break;
		case CLOSED:
			if (millis() >= buttonTime + BUTTON_HOLD_TIME) {
				buttonState = HELD;
			}
			if (pinState) {
				;
			} else {
				buttonState = CHECK_PRESSED;
			}
			break;
		case CHECK_PRESSED:
			if (millis() >= buttonTime + BUTTON_PRESS_TIME) {
				buttonState = PRESSED;
			} else {
				buttonState = OPEN;
			}
			break;
		case PRESSED:
			buttonState = OPEN;
			break;
		case HELD:
			if (!pinState) {
				buttonState = OPEN;
			}
			break;
		default:
			break;
	}
}