#include "Arduino.h"
#include "ExEzButton.h"
#include "Status.h"
#include "RotaryControler.h"
#include "EchoLotSetup.h"
#include "ThreePositionSwitch.h"

static const int trigPin = PB11;
static const int echoPin = PB10;

static const int Pos3Sw_Pin1 = PB13;
static const int Pos3Sw_Pin2 = PB14;
static const int Pos3Sw_Pin3 = PB15;

static const int Measure_Pin = PB6;

static const int RotEnc_Clk_Pin = PB7;
static const int RotEnc_Dta_Pin = PB8;
static const int RotEnc_Switch_Pin = PB9;

Status actualStatus;
Status originStatus;
String oldStatus = "";
String lastDistance = "";

RotaryControler RotaryControler(RotEnc_Dta_Pin, RotEnc_Clk_Pin, RotEnc_Switch_Pin);
ExEzButton MeasureBtn(Measure_Pin, false, 2000);
ThreePositionSwitch ThreePositionSwitch(Pos3Sw_Pin1, Pos3Sw_Pin2, Pos3Sw_Pin3);
Display Display;
EchoLotSetup EchoLotSetup(Display);

//*****************************************************************************************************
//*** Initialization routine. Reads the eeprom first and sets the (potentially new) configured values.
//*****************************************************************************************************
void Initialize() {
	Display.showInitialization();
	EchoLotSetup.initialize();
	delay(50);

	SetActualStatus(IDLE);
}

void setup() {
	Serial.begin(115200);
	delay(500);

	pinMode(RotEnc_Switch_Pin, INPUT);
	pinMode(RotEnc_Dta_Pin, INPUT);
	pinMode(RotEnc_Clk_Pin, INPUT);
	pinMode(trigPin, OUTPUT);
	pinMode(echoPin, INPUT);
	pinMode(Measure_Pin, INPUT);

	MeasureBtn.setDebounceTime(50);
	RotaryControler.setDebounceTime(50);
	ThreePositionSwitch.setDebounceTime(50);

	Display.init();
	SetActualStatus(INITIALIZATION);

	Serial.println("Setup end");
	delay(50);
}

void loop() {

	MeasureBtn.loop();
	RotaryControler.loop();

	switch (actualStatus) {

		case INITIALIZATION:
			Initialize();
			SetActualStatus(IDLE);
			break;

		case CONFIGURATION:
			break;

		case IDLE: {
			if (RotaryControler.isSwitchLongPressed()) {
				Serial.println("RotaryEnc Long SwitchPressed");
				RotaryControler.resetPosition();
			}
			if (RotaryControler.isSwitchPressed()) {
				Serial.println("RotaryEnc SwitchPressed");
			} else if (MeasureBtn.isPressing()) {
				SetActualStatus(MEASURING);
			}

			ThreePositionSwitch.loop();
			int pin = ThreePositionSwitch.getPosition();
//			Serial.print("3-Pos-Schalter Position: ");
//			Serial.println(pin);

			delay(200);
			break;
		}

		case MEASURING:
			if (MeasureBtn.isPressing()) {
				digitalWrite(trigPin, LOW);
				delayMicroseconds(5);
				digitalWrite(trigPin, HIGH);
				delayMicroseconds(20);
				digitalWrite(trigPin, LOW);

				long duration = pulseIn(echoPin, HIGH);
				long distance = duration * 0.03432 / 2;

				Serial.print("Distance:");
				Serial.println(distance);

//				char buf[50];
//				sprintf(buf, "%lu cm", distance);
//
//				if (!lastDistance.equals(buf)) {
//					tft.fillRect(0, 20, 130, 50, ST7735_BLACK);
//					tft.setCursor(0, 50);  // Set position (x,y)
//					tft.setFont(&titillium_web_regular20pt7b);
//					tft.setTextSize(0);  // Set text size. Goes from 0 (the smallest) to 20 (very big)
//					tft.setTextColor(ST7735_CYAN);
//					tft.println(buf);
//					lastDistance = buf;
//				}

				delay(200);
			} else {
//				tft.fillScreen(ST7735_BLACK);
				SetActualStatus(IDLE);
			}
			break;
		default:
			break;
	}
}

void SetActualStatus(Status newStatus) {
	if (actualStatus != newStatus) {
		actualStatus = newStatus;
//		Display.setRefreshScreen();
	}
}