#include "Arduino.h"
#include "EDIPTFT.h"
#include "FT_2018_STW_CAN.h"
#include "FT18_STW_INIT.h"
#include "FT18_STW_DISPLAY.h"

EDIPTFT tft(true,false);
String bezeichnungen[]={"T_mot","T_oil","P_oil","% fa","U_batt","P_wat","T_air",
						"P_b_front","P_b_rear","Error Type","Speed_fl","Speed_fr","Speed"};
//"Drehzahl","P_fuel","Index"
int led_s[] = {led1,led2,led3,led4,led5,led6,led7,led8,led9,led10,led11,led12,led13,led14,led15,led16};

DataBox gear_box(121, 0, 199, 94, 160, 0, EA_SWISS30B, 4, 4, 'C');
DataBox left_box(0, 0, 119, 94, 110, 12, EA_FONT7X12, 3, 8, 'R');
DataBox right_box(201, 0, 320, 94, 310, 12, EA_FONT7X12, 3, 8, 'R');
TireTempBox fl_box(80, 130, 156, 176, 118, 124, EA_FONT7X12, 3, 5, 'C');
TireTempBox fr_box(164, 130, 240, 176, 202, 124, EA_FONT7X12, 3, 5, 'C');
TireTempBox rl_box(80, 184, 156, 230, 118, 178, EA_FONT7X12, 3, 5, 'C');
TireTempBox rr_box(164, 184, 240, 230, 202, 178, EA_FONT7X12, 3, 5, 'C');

void init_display() {
	pinMode(writeprotect, OUTPUT);
	digitalWrite(writeprotect, HIGH);
	pinMode(reset, OUTPUT);
	pinMode(disp_cs, OUTPUT);
	pinMode(MOSI, OUTPUT);
	pinMode(MISO, OUTPUT);
	digitalWrite(disp_cs, HIGH);
	digitalWrite(MOSI, HIGH);
	digitalWrite(MISO, HIGH);
	digitalWrite(reset, LOW);
	digitalWrite(reset,HIGH);
	tft.begin(115200);  // start display communication
	tft.cursorOn(false);
	tft.terminalOn(false);
	tft.setDisplayColor(EA_WHITE,EA_BLACK);
	tft.setTextColor(EA_WHITE,EA_TRANSPARENT);
	tft.setTextSize(5,8);
	tft.clear();
	
	gear_box.update_label(get_label(VAL_GEAR));
	left_box.update_label(get_label(VAL_FIRST_LEFT_BOX));
	right_box.update_label(get_label(VAL_RPM));
}

String get_value(Value val) {
	switch (val) {
	case VAL_GEAR:
		if (Vehicle_data.gear == 0) {
			return "N";
		}
		return String(Vehicle_data.gear);
	case VAL_RPM:
		return String(Vehicle_data.revol);
	case VAL_TT_FL:
		return "00";
	case VAL_TT_FR:
		return "01";
	case VAL_TT_RL:
		return "10";
	case VAL_TT_RR:
		return "11";
	case VAL_LAPTIME:
		return "93.13";
    case VAL_UBATT:
		return String(0.0706949 * Vehicle_data.u_batt, 2);
	case VAL_TMOT:
		return String(Vehicle_data.t_mot - 40);
	case VAL_TAIR:
		return String(Vehicle_data.t_air - 40);
	case VAL_TOIL:
		return String(Vehicle_data.t_oil - 40);
	case VAL_ERR_TYPE:
		return String(Stw_data.error_type);
	case VAL_PWAT:
		return String(0.0514*Vehicle_data.p_wat, 2);
	case VAL_POIL:
		return String(0.0514*Vehicle_data.p_oil, 2);
	case VAL_PBF:
		return String(Vehicle_data.p_brake_front);
	case VAL_PBR:
		return String(Vehicle_data.p_brake_rear);
	case VAL_SPEED_FL:
		return String(Vehicle_data.speed_fl);
	case VAL_SPEED_FR:
		return String(Vehicle_data.speed_fr);
	case VAL_SPEED:
		return String(Vehicle_data.speed);
	default:
		return "???";
	}
}

String get_label(Value val) {
	switch (val) {
	case VAL_GEAR:
		return "GEAR";
	case VAL_RPM:
		return "RPM";
	case VAL_TT_FL:
		return "TEMP FL";
	case VAL_TT_FR:
		return "TEMP FR";
	case VAL_TT_RL:
		return "TEMP RL";
	case VAL_TT_RR:
		return "TEMP RR";
	case VAL_LAPTIME:
		return "LAPTIME";
    case VAL_UBATT:
		return "BATT VOLTAGE";
	case VAL_TMOT:
		return "TEMP ENG";
	case VAL_TAIR:
		return "TEMP AIR";
	case VAL_TOIL:
		return "TEMP OIL";
	case VAL_ERR_TYPE:
		return "ERROR TYPE";
	case VAL_PWAT:
		return "PRESS WAT";
	case VAL_POIL:
		return "PRESS OIL";
	case VAL_PBF:
		return "PRESS BRAKE F";
	case VAL_PBR:
		return "PRESS BRAKE R";
	case VAL_SPEED_FL:
		return "SPEED FL";
	case VAL_SPEED_FR:
		return "SPEED FR";
	case VAL_SPEED:
		return "SPEED";
	default:
		return "???";
	}
}

bool check_alarms() {
	static uint32_t poil_last_valid, tmot_last_valid, toil_last_valid;
	uint32_t now = millis();
	if (Vehicle_data.p_oil >= POIL_ALARM_THRESH || Vehicle_data.speed == 0) {
		poil_last_valid = now;
	}
	if (Vehicle_data.t_mot <= TMOT_ALARM_THRESH || Vehicle_data.t_mot == TMOT_SAFE_VALUE) {
		tmot_last_valid = now;
	}
	if (Vehicle_data.t_oil <= TOIL_ALARM_THRESH) {
		toil_last_valid = now;
	}
	bool poil_alarm = now - poil_last_valid >= POIL_ALARM_TIME;
	bool tmot_alarm = now - tmot_last_valid >= TMOT_ALARM_TIME;
	bool toil_alarm = now - toil_last_valid >= TOIL_ALARM_TIME;
	bool alarm_active = poil_alarm || tmot_alarm || toil_alarm;

	if (alarm_active) {
		String alarm_text = "";
		if (poil_alarm) alarm_text += "PO";
		if (tmot_alarm) alarm_text += "TM";
		if (toil_alarm) alarm_text += "TO";
		alarm(alarm_text);
	}

	return alarm_active;
}

bool check_enc_displays() {
	static uint8_t trc_old, mode_old;
	static bool display_trc, display_mode;
	static uint32_t display_trc_begin, display_mode_begin;

	return check_display(trc_old, Stw_data.trc, display_trc, display_trc_begin, "ARB") ||
		check_display(mode_old, Stw_data.mode, display_mode, display_mode_begin, "MODE");
}

bool check_display(uint8_t& val_old, uint8_t val_new, bool& active, uint32_t& begin, const String& title) {
	if (val_old != val_new) {
		active = true;
		begin = millis();
		val_old = val_new;
		tft.clear();
		tft.fillDisplayColor(EA_RED);
		tft.setTextColor(EA_WHITE, EA_RED);
		tft.setTextSize(7,8);
		String text = title + ":" + val_new;
		char text_arr[16];
		text.toCharArray(text_arr, 16);
		tft.drawText(15, 68, 'C', text_arr);
	} else if (active && millis() - begin > ENC_DISPLAY_TIME) {
		tft.setTextColor(EA_WHITE, EA_TRANSPARENT);
		tft.clear();
		active = false;
	}

	return active;
}

void update_display(){
	static DisplayPage page = PAGE_DRIVER;
	static uint32_t last_cleared;
	static bool cleared = true;

	if (check_alarms()) {
		cleared = true;
		return;
	}
	if (tft.disconnected) {
		return;
	}

	if (check_enc_displays()) {
		cleared = true;
		return;
	}

	uint32_t now = millis();
	// Both buttons have to be pressed at the same time, but we also use the
	// debounced rises to ensure we don't keep toggling between the pages
	if (Stw_data.buttonState1 && Stw_data.buttonState4 &&
			(Stw_data.button1_rises > 0|| Stw_data.button4_rises > 0)){
		Stw_data.button1_rises = 0;
		Stw_data.button4_rises = 0;
		page = (DisplayPage) ((page + 1) % DISPLAY_PAGES);
		tft.clear();
		last_cleared = now;
		cleared = true;
	}

	if (now - last_cleared >= DISP_CLEAR_INTERVAL) {
		tft.clear();
		last_cleared = now;
		cleared = true;
	}

	if (page == PAGE_DRIVER) {
		if (cleared) {
			redraw_page_driver();
			cleared = false;
		} else {
			update_page_driver();
		}
	} else {
		if (cleared) {
			redraw_page_testing();
			cleared = false;
		} else {
			update_page_testing();
		}
	}
}

void alarm(String textstr){
	uint8_t x = 1;;
	char text[7];
	textstr.toCharArray(text,7);
	tft.setTextSize(8,8);
	while(x==1){
		if(!tft.disconnected){
			tft.setTextColor(EA_BLACK,EA_RED);
			tft.fillDisplayColor(EA_RED);
			tft.drawText(5,68,'L',text);
		}
		for (int j = 0; j < 16; j++){
			digitalWrite(led_s[j], HIGH);
		}
		delay(100);
		if(!tft.disconnected){
			tft.setTextColor(EA_BLACK,EA_WHITE);
			tft.fillDisplayColor(EA_WHITE);
			tft.drawText(5,68,'L',text);
		}
		for (int j = 0; j < 16; j++){
			digitalWrite(led_s[j], LOW);
		}
		delay(100);
		if(Stw_data.buttonState1 & Stw_data.buttonState4){
			x=0;
			tft.setTextColor(EA_WHITE,EA_TRANSPARENT);
		}
	}
}

void redraw_page_driver() {
	// Boxes
	tft.drawLine(0, 110, 320, 110);
	tft.drawLine(120, 0, 120, 110);
	tft.drawLine(200, 0, 200, 110);

	// Tire temperature cross
	tft.drawLine(80, 180, 240, 180);
	tft.drawLine(160, 130, 160, 230);

	// Boxes
	gear_box.redraw();
	left_box.redraw();
	right_box.redraw();
	fl_box.redraw();
	fr_box.redraw();
	rl_box.redraw();
	rr_box.redraw();
}

void update_page_driver() {
	static Value left_box_value = VAL_FIRST_LEFT_BOX;
	if (Stw_data.button4_rises > 0) {
		Stw_data.button4_rises--;
		if (left_box_value == VAL_LAST) {
			left_box_value = VAL_FIRST_LEFT_BOX;
		} else {
			left_box_value = (Value) (left_box_value + 1);
		}
		left_box.update_label(get_label(left_box_value));
	if (Stw_data.button1_rises > 0) {
		Stw_data.button1_rises--;
		if (left_box_value == VAL_FIRST_LEFT_BOX) {
			left_box_value = VAL_LAST;
		} else {
			left_box_value = (Value) (left_box_value - 1);
		}
		left_box.update_label(get_label(left_box_value));
	}

	gear_box.update_value(get_value(VAL_GEAR));
	left_box.update_value(get_value(left_box_value));
	right_box.update_value(get_value(VAL_RPM));
	fl_box.update_value(2);
	fr_box.update_value(55);
	rl_box.update_value(65);
	rr_box.update_value(90);
}

void redraw_page_testing() {

}

void update_page_testing() {}

DataBox::DataBox(int x1, int y1, int x2, int y2, int text_x, int text_y, int font,
        int size_x, int size_y, uint8_t justification)
	: x1{x1}, y1{y1}, x2{x2}, y2{y2}, text_x{text_x}, text_y{text_y},
		font{font}, size_x{size_x}, size_y{size_y},
		justification{justification}, value{""}, label{""} {}

void DataBox::update_value(String val_new) {
	if (!val_new.equals(value)) {
		value = val_new;
		redraw_value();
	}
}

void DataBox::update_label(String label_new) {
	if (!label_new.equals(label)) {
		label = label_new;
		redraw_label();
	}
}

void DataBox::redraw() {
	redraw_value();
	redraw_label();
}

void DataBox::redraw_value() {
	tft.setTextFont(font);
	tft.setTextSize(size_x, size_y);
	Serial.println("Redrawing value:");
	tft.clearRect(x1, y1, x2, y2);
	tft.drawText(text_x, text_y, justification, value.c_str());
}

void DataBox::redraw_label() {
	tft.setTextFont(EA_FONT7X12);
	tft.setTextSize(1, 1);
	Serial.println("Redrawing label:");
	tft.clearRect(x1, y2 + 1, x2, y2 + 13);
	tft.drawText((x1 + x2) / 2, y2 + 1, 'C', label.c_str());
}

TireTempBox::TireTempBox(int x1, int y1, int x2, int y2, int text_x, int text_y,
        int font, int size_x, int size_y, uint8_t justification)
	: DataBox{x1, y1, x2, y2, text_x, text_y, font, size_x, size_y, justification}, num_value{-1} {}

void TireTempBox::update_value(int val_new) {
	if (val_new != num_value) {
		num_value = val_new;
		if (val_new < TT_THRESH1) {
			color = TT_COL0;
		} else if (val_new < TT_THRESH2) {
			color = TT_COL1;
		} else if (val_new < TT_THRESH3) {
			color = TT_COL2;
		} else {
			color = TT_COL3;
		}
		String val_str = String(val_new);
		if (val_str.length() == 1) {
			val_str = "  " + val_str;
		} else if (val_str.length() == 2) {
			val_str = " " + val_str;
		}
		DataBox::update_value(val_str);
	}
}

void TireTempBox::redraw_value() {
	tft.setTextFont(font);
	tft.setTextSize(size_x, size_y);
	tft.drawRectf(x1, y1, x2, y2, color);
	tft.drawText(text_x, text_y, justification, value.c_str());
}

void TireTempBox::redraw_label() {}