#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 = "";
int environmentPostion = 0;

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();
}

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() {
	switch (actualStatus) {

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

		case CONFIGURATION:
			rotaryControler.loop();
			if (originStatus != CONFIGURATION) {
				display.setRefreshScreen();
			}
			display.showFrame(actualStatus);

			echoLotSetup.onRotaryControlerTurn(rotaryControler.getEncoderMove());

			if (rotaryControler.isSwitchLongPressed()) {
				Serial.println("RotaryEnc Long SwitchPressed");
				delay(50);
				// TODO: Save the edited config values
				SetActualStatus(IDLE);
			}

			originStatus = CONFIGURATION;
			break;

		case IDLE: {
			measureBtn.loop();
			rotaryControler.loop();
			threePositionSwitch.loop();

			if (originStatus == CONFIGURATION || originStatus == INITIALIZATION) {
				display.setRefreshScreen();
				display.showFrame(actualStatus);
			}

			if (rotaryControler.isSwitchLongPressed()) {
				Serial.println("RotaryEnc Long SwitchPressed");
				delay(50);
				SetActualStatus(CONFIGURATION);
			}

			if (measureBtn.isPressing()) {
				SetActualStatus(MEASURING);
			}

			environmentPostion = threePositionSwitch.getPosition();
			display.setEnvironment(environmentPostion);

			originStatus = IDLE;
			delay(50);
			break;
		}

		case MEASURING:
			measureBtn.loop();
			if (measureBtn.isPressing()) {
				display.runMeasureAnimation(true);
				digitalWrite(trigPin, LOW);
				delayMicroseconds(5);
				digitalWrite(trigPin, HIGH);
				delayMicroseconds(20);
				digitalWrite(trigPin, LOW);

				//TODO: calculate real distanz according settings
				uint32_t duration = pulseIn(echoPin, HIGH);
				uint32_t distance = calculateDistance(duration);

				display.setDistance(distance);
				delay(200);
			} else {
				display.setDistance(0);
				display.runMeasureAnimation(false);
				SetActualStatus(IDLE);
			}
			originStatus = MEASURING;
			break;
		default:
			break;
	}
}

void SetActualStatus(Status newStatus) {
	if (actualStatus != newStatus) {
		actualStatus = newStatus;
	}
}

uint32_t calculateDistance(uint32_t duration) {
	if (environmentPostion == 1) {
		float airTemp = echoLotSetup.getAirTemp();
		float speedAir = echoLotSetup.getSonicSpeedAir();

		return ((speedAir + 0.6 * airTemp) / 1000 * duration) / 2;

	} else {
		//cS = (1449 + 4,6T - 0,05T^2 + 1,4(S-35)) T=Tempeature, S= Salt in PerMille
		float waterTemp = echoLotSetup.getWaterTemp();
		int saltLvl = environmentPostion == 2 ? 0 : echoLotSetup.getSaltPromilleWater();
		int speedWater = 1449 + 4.6 * waterTemp - 0.05 * pow(waterTemp, 2) + 1.4 * (saltLvl - 35);

		Serial.println("duration: " + String(duration));
		Serial.println("saltLvl: " + String(saltLvl));
		Serial.println("speedWater: " + String(speedWater));
		Serial.println("distance: " + String((float(speedWater) / 1000 * duration) / 2));

		return (float(speedWater) / 1000 * duration) / 2;
	}
}