#include "HMI.h"
#include <Controllino.h>
#include <Adafruit_ILI9341.h>
#include <Encoder.h>
#include "Colours.h"
#include "Bitmaps.h"
#include "PLC.h"

#define LCD_CS CONTROLLINO_PIN_HEADER_DIGITAL_OUT_14
#define LCD_DC CONTROLLINO_UART_TX
#define ENCODER_A CONTROLLINO_IN0
#define ENCODER_B CONTROLLINO_IN1
#define ENCODER_BUTTON CONTROLLINO_PIN_HEADER_DIGITAL_OUT_12

#define BACKGROUND_COLOR TFT_WHITE
#define FOREGROUND_COLOR TFT_BLACK

// screens
enum screens {
	SCREEN_INFO,
	SCREEN_MENU_MAIN,
	SCREEN_MENU_ELECTRONICS_CALIBRATION,
	SCREEN_MENU_ARM_CALIBRATION,
	SCREEN_MENU_AGING_IDLE,
	SCREEN_MENU_AGING_RUNNING,
	SCREEN_MENU_AGING_PAUSED,
	SCREEN_MENU_MANUAL_CONTROL,
	SCREEN_MENU_SERIAL_NUMBER
};
screens currentScreen = SCREEN_INFO;

// space between selection indication box and text
#define BOX_TEXT_STANDOFF 5

// common menu variables
#define MENU_SPACING 30
#define MENU_FONT_WIDTH 10
#define MENU_FONT_HEIGHT 14
const uint16_t menuCorner[2] = {10, 2*10+MENU_FONT_HEIGHT + (MENU_SPACING-MENU_FONT_HEIGHT)};
uint8_t menuSelectedOption = 0;

// main menu
#define MENU_MAIN_NUM_OPTIONS 6
const char* menuMainOptions[] = {"<-", "Zahorovani", "Kalibrace elektroniky", "Kalibrace ramene", "Manualni ovladani", "LOG"};

// electronics calibration menu
#define MENU_ELECTRONICS_CALIBRATION_NUM_OPTIONS 3
const char* menuElectronicsCalibrationOptions[] = {"<-", "Sloupec 0", "Sloupec 1"};

// arm calibration menu
#define MENU_ARM_CALIBRATION_NUM_OPTIONS 5
const char* menuArmCalibrationOptions[] = {"<-", "Krabice", "Sloupec", "AOI", "Indukce"};

// aging menu while aging is idle
#define MENU_AGING_IDLE_NUM_OPTIONS 3
const char* menuAgingIdleOptions[] = {"<-", "Start R", "Start F"};

// aging menu while aging is running
#define MENU_AGING_RUNNING_NUM_OPTIONS 3
const char* menuAgingRunningOptions[] = {"<-", "Pauza", "Stop"};

// aging menu while aging is paused
#define MENU_AGING_PAUSED_NUM_OPTIONS 3
const char* menuAgingPausedOptions[] = {"<-", "Obnovit", "Stop"};

// manual control menu
#define MENU_MANUAL_CONTROL_NUM_OPTIONS 2
const char* menuManualControlOptions[] = {"<-", "Seriove cislo"};

// serial number menu
#define MENU_SERIAL_NUMBER_NUM_OPTIONS 2
const char* menuSerialNumberOptions[] = {"<-", "Rozpoznat"};

// comms info section - vertical text block
#define COMMS_INFO_SPACING 15
#define COMMS_INFO_FONT_WIDTH 5
#define COMMS_INFO_FONT_HEIGHT 7
const uint16_t commsInfoCorner[2] = {240, 180};
#define COMMS_INFO_NUM_ELEMENTS 4
const char* commsInfoText[] = {"Sloupec 0", "Sloupec 1", "Robot", "AOI"};
#define COMMS_INFO_FRAME_WIDTH 80

bool commsStatus[4];

int32_t lastEncCount = 0;

xArm* armPointer;
AgingColumn* column0Pointer;
AgingColumn* column1Pointer;
AOI* aoiPointer;

Adafruit_ILI9341 LCD = Adafruit_ILI9341(LCD_CS, LCD_DC);
Encoder encoder(ENCODER_A, ENCODER_B);

void changeSelectedOption(bool up);
uint16_t getTextWidth(char* text);

agingStates lastAgingState = AGING_STATE_IDLE;
agingSubstates lastAgingSubstate = AGING_SUBSTATE_FILLING;
nixieTypes lastNixieType = NIXIE_R;
uint32_t lastSerialNumber = 0;
bool serialNumberRedrawn = false;

void redrawStateInfo() {
	if (currentScreen != SCREEN_INFO) return;

	LCD.fillRect(150, 31, 170, 20, BACKGROUND_COLOR);
	LCD.setTextSize(2);
	LCD.setCursor(150,33);

	uint16_t textWidth = 0;
	switch (agingState) {
		case AGING_STATE_IDLE:
			LCD.setTextColor(TFT_BLACK);
			LCD.print("IDLE");
			return;
		break;
		case AGING_STATE_ERROR:
			LCD.setTextColor(TFT_RED);
			LCD.print("ERROR");
			return;
		break;
		case AGING_STATE_RUNNING:
			LCD.setTextColor(TFT_DARKGREEN);
			LCD.print("RUN");
			textWidth = getTextWidth("RUN");
		break;
		case AGING_STATE_PAUSED:
			LCD.setTextColor(TFT_ORANGE);
			LCD.print("PAUZA");
			textWidth = getTextWidth("PAUZA");
		break;
	}

	LCD.setTextColor(FOREGROUND_COLOR);
	LCD.setTextSize(1);
	LCD.setCursor(150+textWidth+15, 31);
	switch (nixieType) {
		case NIXIE_R:
			LCD.print("Digitrony R");
		break;
		case NIXIE_F:
			LCD.print("Digitrony F");
		break;
	}

	LCD.setCursor(150+textWidth+15, 42);
	switch (agingSubstate) {
		case AGING_SUBSTATE_FILLING:
			LCD.print("Plneni pozic");
		break;
		case AGING_SUBSTATE_EMPTYING:
			LCD.print("Vytazeni pozic");
		break;
		case AGING_SUBSTATE_CYCLE_1:
			LCD.print("Cyklus 1");
		break;
		case AGING_SUBSTATE_CYCLE_2:
			LCD.print("Cuklus 2");
		break;
		case AGING_SUBSTATE_HG_RELEASE:
			LCD.print("Uvolneni rtuti");
		break;
	}
}

void redrawCommsStatus() {
	if (currentScreen != SCREEN_INFO) return;

	uint16_t x = commsInfoCorner[0] + COMMS_INFO_FRAME_WIDTH - 3*COMMS_INFO_FONT_WIDTH - 1;
	for (uint8_t i = 0; i < 4; i++) {
		uint16_t y = commsInfoCorner[1] + i*COMMS_INFO_SPACING - 1;
		LCD.fillRect(x, y, 10, 10, BACKGROUND_COLOR);

		if (commsStatus[i]) LCD.drawBitmap(x, y, tickBitmap, 10, 10, TFT_DARKGREEN);
		else LCD.drawBitmap(x, y, crossBitmap, 9, 9, TFT_RED);
	}
}

void redrawSerialNumber(bool waiting=false) {
	LCD.fillRect(150+12, 80, 160, 14, BACKGROUND_COLOR);
	LCD.setCursor(150, 80);
	if (waiting) LCD.print("rozpoznavam...");
	else {
		if (aoiPointer->lastSerialNumber == 0) LCD.print("chyba");
		else {
			LCD.print("#");
			LCD.print(aoiPointer->lastSerialNumber);
		}
	}
}

uint16_t getTextWidth(char* text) {
	return strlen(text) * MENU_FONT_WIDTH + (strlen(text)-1)*2;
}

void printVerticalTextArray(uint16_t corner[2], char* text[], uint8_t arraySize, uint8_t spacing) {
	for (uint8_t i = 0; i < arraySize; i++) {
		LCD.setCursor(corner[0], corner[1] + spacing*i);
		LCD.print(text[i]);
	}
}

void drawRectAroundText(uint16_t x, uint16_t y, char* text, uint8_t fontHeight, uint16_t color) {
	LCD.drawRoundRect(
	x - BOX_TEXT_STANDOFF,
	y - BOX_TEXT_STANDOFF,
	getTextWidth(text) + 2*BOX_TEXT_STANDOFF,
	fontHeight + 2*BOX_TEXT_STANDOFF,
	3,
	color);
}

void drawHeader(char* text) {
	LCD.setCursor((320-getTextWidth(text))/2, 10);
	LCD.print(text);
	uint16_t linePosY = 2*10 + MENU_FONT_HEIGHT;
	LCD.drawFastHLine(0, linePosY, 320, FOREGROUND_COLOR);
	LCD.drawFastHLine(0, linePosY+1, 320, FOREGROUND_COLOR);
}

void changeScreen(uint8_t screen) {
	currentScreen = screen;

	LCD.fillScreen(BACKGROUND_COLOR);
	LCD.setTextColor(FOREGROUND_COLOR, BACKGROUND_COLOR);
	switch (screen) {
		case SCREEN_INFO:{
			// draw name
			LCD.setTextSize(2);
			LCD.setCursor(10,10);
			LCD.print("Ager 3.0");
			LCD.drawRoundRect(10 - BOX_TEXT_STANDOFF, 10 - BOX_TEXT_STANDOFF, getTextWidth("Ager 3.0") + 2*BOX_TEXT_STANDOFF, 14 + 2*BOX_TEXT_STANDOFF, 7, FOREGROUND_COLOR);
			LCD.drawRoundRect(10 - BOX_TEXT_STANDOFF - 1, 10 - BOX_TEXT_STANDOFF - 1, getTextWidth("Ager 3.0") + 2*BOX_TEXT_STANDOFF + 2, 14 + 2*BOX_TEXT_STANDOFF + 2, 8, FOREGROUND_COLOR);
			// draw xArm icon
			LCD.drawBitmap(0, 35, xArmBitmap, 136, 200, FOREGROUND_COLOR);
			// draw comms info table
			const uint8_t commsFrameStadoff = (COMMS_INFO_SPACING-COMMS_INFO_FONT_HEIGHT)/2;
			LCD.setTextSize(1);
			LCD.setCursor(commsInfoCorner[0]+(COMMS_INFO_FRAME_WIDTH-10*(COMMS_INFO_FONT_WIDTH+1))/2-commsFrameStadoff, commsInfoCorner[1]-COMMS_INFO_SPACING);
			LCD.print("Komunikace");
			printVerticalTextArray(commsInfoCorner, commsInfoText, COMMS_INFO_NUM_ELEMENTS, COMMS_INFO_SPACING);
			for (uint8_t i = 0; i <= COMMS_INFO_NUM_ELEMENTS; i++) {
				LCD.drawFastHLine(commsInfoCorner[0] - commsFrameStadoff, commsInfoCorner[1] - commsFrameStadoff + i*COMMS_INFO_SPACING, COMMS_INFO_FRAME_WIDTH, FOREGROUND_COLOR);
			}
			LCD.drawFastVLine(commsInfoCorner[0] - commsFrameStadoff, commsInfoCorner[1] - commsFrameStadoff, COMMS_INFO_NUM_ELEMENTS*COMMS_INFO_SPACING, FOREGROUND_COLOR);
			LCD.drawFastVLine(commsInfoCorner[0] - commsFrameStadoff + COMMS_INFO_FRAME_WIDTH, commsInfoCorner[1] - commsFrameStadoff, COMMS_INFO_NUM_ELEMENTS*COMMS_INFO_SPACING+1, FOREGROUND_COLOR);
			LCD.drawFastVLine(commsInfoCorner[0] - commsFrameStadoff + COMMS_INFO_FRAME_WIDTH - COMMS_INFO_SPACING, commsInfoCorner[1] - commsFrameStadoff, COMMS_INFO_NUM_ELEMENTS*COMMS_INFO_SPACING+1, FOREGROUND_COLOR);
			// draw current comms status
			redrawCommsStatus();
			// draw current machine state info
			LCD.setTextSize(2);
			LCD.setCursor(150,10);
			LCD.print("STAV STROJE");
			redrawStateInfo();
			break;}
		case SCREEN_MENU_MAIN:
			menuSelectedOption = 0;
			LCD.setTextSize(2);
			drawHeader("Hlavni menu");
			printVerticalTextArray(menuCorner, menuMainOptions, MENU_MAIN_NUM_OPTIONS, MENU_SPACING);
			changeSelectedOption(0);
		break;
		case SCREEN_MENU_ELECTRONICS_CALIBRATION:
			menuSelectedOption = 0;
			LCD.setTextSize(2);
			drawHeader("Kalibrace elektroniky");
			printVerticalTextArray(menuCorner, menuElectronicsCalibrationOptions, MENU_ELECTRONICS_CALIBRATION_NUM_OPTIONS, MENU_SPACING);
			changeSelectedOption(0);
		break;
		case SCREEN_MENU_ARM_CALIBRATION:
			menuSelectedOption = 0;
			LCD.setTextSize(2);
			drawHeader("Kalibrace ramene");
			printVerticalTextArray(menuCorner, menuArmCalibrationOptions, MENU_ARM_CALIBRATION_NUM_OPTIONS, MENU_SPACING);
			changeSelectedOption(0);
		break;
		case SCREEN_MENU_AGING_IDLE:
			menuSelectedOption = 0;
			LCD.setTextSize(2);
			drawHeader("Zahorovani neprobiha");
			printVerticalTextArray(menuCorner, menuAgingIdleOptions, MENU_AGING_IDLE_NUM_OPTIONS, MENU_SPACING);
			changeSelectedOption(0);
		break;
		case SCREEN_MENU_AGING_RUNNING:
			menuSelectedOption = 0;
			LCD.setTextSize(2);
			drawHeader("Zahorovani bezi");
			printVerticalTextArray(menuCorner, menuAgingRunningOptions, MENU_AGING_RUNNING_NUM_OPTIONS, MENU_SPACING);
			changeSelectedOption(0);
		break;
		case SCREEN_MENU_AGING_PAUSED:
			menuSelectedOption = 0;
			LCD.setTextSize(2);
			drawHeader("Zahorovani pozastaveno");
			printVerticalTextArray(menuCorner, menuAgingPausedOptions, MENU_AGING_PAUSED_NUM_OPTIONS, MENU_SPACING);
			changeSelectedOption(0);
		break;
		case SCREEN_MENU_MANUAL_CONTROL:
			menuSelectedOption = 0;
			LCD.setTextSize(2);
			drawHeader("Manualni ovladani");
			printVerticalTextArray(menuCorner, menuManualControlOptions, MENU_MANUAL_CONTROL_NUM_OPTIONS, MENU_SPACING);
			changeSelectedOption(0);
		break;
		case SCREEN_MENU_SERIAL_NUMBER:
			menuSelectedOption = 0;
			LCD.setTextSize(2);
			drawHeader("Seriove cislo");
			printVerticalTextArray(menuCorner, menuSerialNumberOptions, MENU_SERIAL_NUMBER_NUM_OPTIONS, MENU_SPACING);
			changeSelectedOption(0);
			LCD.setCursor(150, 50);
			LCD.print("Posledni SC:");
			redrawSerialNumber();
		break;
	}
}

void changeMenuSelection(char* menu[], uint8_t numOptions, bool up) {
	// erase previous selection box
	drawRectAroundText(menuCorner[0], menuCorner[1] + (menuSelectedOption * MENU_SPACING), menu[menuSelectedOption], MENU_FONT_HEIGHT, BACKGROUND_COLOR);
	// change selection
	if (up && menuSelectedOption < numOptions-1) menuSelectedOption++;
	else if (!up && menuSelectedOption > 0) menuSelectedOption--;
	// draw new selection box
	drawRectAroundText(menuCorner[0], menuCorner[1] + (menuSelectedOption * MENU_SPACING), menu[menuSelectedOption], MENU_FONT_HEIGHT, FOREGROUND_COLOR);
}

void changeSelectedOption(bool up) {
	switch (currentScreen) {
		case SCREEN_INFO:
		break;
		case SCREEN_MENU_MAIN:
			changeMenuSelection(menuMainOptions, MENU_MAIN_NUM_OPTIONS, up);
		break;
		case SCREEN_MENU_ELECTRONICS_CALIBRATION:
			changeMenuSelection(menuElectronicsCalibrationOptions, MENU_ELECTRONICS_CALIBRATION_NUM_OPTIONS, up);
		break;
		case SCREEN_MENU_ARM_CALIBRATION:
			changeMenuSelection(menuArmCalibrationOptions, MENU_ARM_CALIBRATION_NUM_OPTIONS, up);
		break;
		case SCREEN_MENU_AGING_IDLE:
			changeMenuSelection(menuAgingIdleOptions, MENU_AGING_IDLE_NUM_OPTIONS, up);
		break;
		case SCREEN_MENU_AGING_RUNNING:
			changeMenuSelection(menuAgingRunningOptions, MENU_AGING_RUNNING_NUM_OPTIONS, up);
		break;
		case SCREEN_MENU_AGING_PAUSED:
			changeMenuSelection(menuAgingPausedOptions, MENU_AGING_PAUSED_NUM_OPTIONS, up);
		break;
		case SCREEN_MENU_MANUAL_CONTROL:
			changeMenuSelection(menuManualControlOptions, MENU_MANUAL_CONTROL_NUM_OPTIONS, up);
		break;
		case SCREEN_MENU_SERIAL_NUMBER:
			changeMenuSelection(menuSerialNumberOptions, MENU_SERIAL_NUMBER_NUM_OPTIONS, up);
		break;
	}
}

void confirmOption() {
	switch (currentScreen) {
		case SCREEN_INFO:
			changeScreen(SCREEN_MENU_MAIN);
		break;
		case SCREEN_MENU_MAIN:
			switch (menuSelectedOption) {
				case 0:
					changeScreen(SCREEN_INFO);
				break;
				case 1:
					switch (agingState) {
						case AGING_STATE_IDLE:
							changeScreen(SCREEN_MENU_AGING_IDLE);
						break;
						case AGING_STATE_RUNNING:
							changeScreen(SCREEN_MENU_AGING_RUNNING);
						break;
						case AGING_STATE_PAUSED:
							changeScreen(SCREEN_MENU_AGING_PAUSED);
						break;
						case AGING_STATE_ERROR:
							// screen unavailable
						break;
					}
				break;
				case 2:
					if (agingState == AGING_STATE_IDLE || agingState == AGING_STATE_PAUSED) changeScreen(SCREEN_MENU_ELECTRONICS_CALIBRATION);
				break;
				case 3:
					if (agingState == AGING_STATE_IDLE || agingState == AGING_STATE_PAUSED) changeScreen(SCREEN_MENU_ARM_CALIBRATION);
				break;
				case 4:
					changeScreen(SCREEN_MENU_MANUAL_CONTROL);
				break;
			}
		break;
		case SCREEN_MENU_ELECTRONICS_CALIBRATION:
			switch (menuSelectedOption) {
				case 0:
					changeScreen(SCREEN_MENU_MAIN);
				break;
				case 1:
				break;
			}
		break;
		case SCREEN_MENU_ARM_CALIBRATION:
			switch (menuSelectedOption) {
				case 0:
					changeScreen(SCREEN_MENU_MAIN);
				break;
				case 1:
				break;
			}
		break;
		case SCREEN_MENU_AGING_IDLE:
			switch (menuSelectedOption) {
				case 0:
					changeScreen(SCREEN_MENU_MAIN);
				break;
				case 1:
					changeNixieType(NIXIE_R);
					startAging(-1);
				break;
				case 2:
					changeNixieType(NIXIE_F);
					startAging(-1);
				break;
			}
		break;
		case SCREEN_MENU_AGING_RUNNING:
			switch (menuSelectedOption) {
				case 0:
					changeScreen(SCREEN_MENU_MAIN);
				break;
				case 1:
					pauseAging();
				break;
				case 2:
					stopAging();
				break;
			}
		break;
		case SCREEN_MENU_AGING_PAUSED:
			switch (menuSelectedOption) {
				case 0:
					changeScreen(SCREEN_MENU_MAIN);
				break;
				case 1:
					resumeAging();
				break;
				case 2:
					stopAging();
				break;
			}
		break;
		case SCREEN_MENU_MANUAL_CONTROL:
			switch (menuSelectedOption) {
				case 0:
					changeScreen(SCREEN_MENU_MAIN);
				break;
				case 1:
					changeScreen(SCREEN_MENU_SERIAL_NUMBER);
				break;
			}
		break;
		case SCREEN_MENU_SERIAL_NUMBER:
			switch (menuSelectedOption) {
				case 0:
					changeScreen(SCREEN_MENU_MANUAL_CONTROL);
				break;
				case 1:
					aoiPointer->recognizeSerialNumber();
					serialNumberRedrawn = false;
					redrawSerialNumber(true);
				break;
			}
		break;
	}
}

void updateScreens() {
	switch (currentScreen) {
		case SCREEN_INFO: {
			// comms status change check
			bool commsStatusChanged = false;
			if (commsStatus[0] != column0Pointer->commsOk) {
				commsStatusChanged = true;
				commsStatus[0] = column0Pointer->commsOk;
			}
			if (commsStatus[1] != column1Pointer->commsOk) {
				commsStatusChanged = true;
				commsStatus[1] = column1Pointer->commsOk;
			}
			if (commsStatus[2] != armPointer->taskRunning) {
				commsStatusChanged = true;
				commsStatus[2] = armPointer->taskRunning;
			}
			if (commsStatus[3] != aoiPointer->commsOk) {
				commsStatusChanged = true;
				commsStatus[3] = aoiPointer->commsOk;
			}
			if (commsStatusChanged) redrawCommsStatus();
		}
		break;
		case SCREEN_MENU_SERIAL_NUMBER:
			// serial number change check
			if (aoiPointer->lastSerialNumber != lastSerialNumber || (aoiPointer->commandDone && !serialNumberRedrawn)) {
				redrawSerialNumber();
				lastSerialNumber = aoiPointer->lastSerialNumber;
				serialNumberRedrawn = true;
			}
		break;
	}

	// state info change check
	bool stateInfoChanged = false;
	if (lastAgingState != agingState) {
		stateInfoChanged = true;
		lastAgingState = agingState;
	}
	if (lastAgingSubstate != agingSubstate) {
		stateInfoChanged = true;
		lastAgingSubstate = agingSubstate;
	}
	if (lastNixieType != nixieType) {
		stateInfoChanged = true;
		lastNixieType = nixieType;
	}
	if (stateInfoChanged) {
		redrawStateInfo();
		if (currentScreen != SCREEN_INFO && currentScreen != SCREEN_MENU_MAIN) changeScreen(SCREEN_INFO);
	}
}

HMI::HMI(xArm* xarm, AgingColumn* column0, AgingColumn* column1, AOI* aoi) {
	armPointer = xarm;
	column0Pointer = column0;
	column1Pointer = column1;
	aoiPointer = aoi;
}

void HMI::init() {
	LCD.begin();
	LCD.setRotation(1);
	LCD.setTextColor(FOREGROUND_COLOR, BACKGROUND_COLOR);
	LCD.setTextWrap(0);
	changeScreen(SCREEN_INFO);
}

bool encoderPressProcessed = true;

void HMI::poll() {
	// encoder actions check
	int32_t newEncCount = encoder.read()/4;
	if (digitalRead(ENCODER_BUTTON)) encoderPressProcessed = false;
	if (!encoderPressProcessed && !digitalRead(ENCODER_BUTTON)) {
		confirmOption();
		encoderPressProcessed = true;
	}
	else {	// only process encoder rotation if it has not been pressed, otherwise the rotation is ignored
		if (newEncCount > lastEncCount) {	// rotated right
			changeSelectedOption(1);
		}
		else if (newEncCount < lastEncCount) {	// rotated left
			changeSelectedOption(0);
		}
	}
	lastEncCount = newEncCount;

	// update screens with dynamic info
	updateScreens();
}
