/*
 * RouterSetup.cpp
 *
 *  Created on: 25.01.2022
 *      Author: FSmilari
 */

#include "RouterSetup.h"

#include <HardwareSerial.h>
#include <WString.h>


/******************
 ** Constructors
 *****************/
RouterSetup::RouterSetup(Display &_display) : display(_display) {
	doInitialization = true;
	configStepIndex = 0;
}

/******************
 ** Public methods
 *****************/
float RouterSetup::getSpeed() const {
	return speed;
}

void RouterSetup::setSpeed(float speed) {
	this->speed = speed;
}

float RouterSetup::getAcceleration() const {
	return accelleration;
}

void RouterSetup::setAcceleration(float accelleration) {
	this->accelleration = accelleration;
}

uint8_t RouterSetup::getEncoderSpeedFast() const {
	return encoderSpeedFast;
}

void RouterSetup::setEncoderSpeedFast(uint8_t encoderSpeedFast) {
	this->encoderSpeedFast = encoderSpeedFast;
}

uint8_t RouterSetup::getEncoderSpeedSlow() const {
	return encoderSpeedSlow;
}

void RouterSetup::setEncoderSpeedSlow(uint8_t encoderSpeedSlow) {
	this->encoderSpeedSlow = encoderSpeedSlow;
}

float RouterSetup::getLevelHeightDiving() const {
	return levelHeightDiving;
}

void RouterSetup::setLevelHeightDiving(float levelHeightDiving) {
	this->levelHeightDiving = levelHeightDiving;
}

float RouterSetup::getPitch() const {
	return pitch;
}

void RouterSetup::setPitch(float pitch) {
	this->pitch = pitch;
}

uint16_t RouterSetup::getStepsPerRev() const {
	return stepsPerRev;
}

void RouterSetup::setStepsPerRev(uint16_t stepsPerRev) {
	this->stepsPerRev = stepsPerRev;
}

float RouterSetup::getToolLenghtSensorHeight() const {
	return toolLenghtSensorHeight;
}

void RouterSetup::setToolLenghtSensorHeight(float toolLenghtSensorHeight) {
	this->toolLenghtSensorHeight = toolLenghtSensorHeight;
}

bool RouterSetup::isToolChangOnPowerOn() const {
	return toolChangeOnPowerOn;
}

void RouterSetup::setToolChangOnPowerOn(bool toolChangOnPowerOn) {
	this->toolChangeOnPowerOn = toolChangOnPowerOn;
}


void RouterSetup::clear() {
	speed = 0;
	accelleration = 0;
	stepsPerRev = 0;
	pitch = 0;
	toolLenghtSensorHeight = 0;
	encoderSpeedSlow = 0;
	encoderSpeedFast = 0;
	levelHeightDiving = 0;
	toolChangeOnPowerOn = true;
}


/**
 *  Reads the RouterSetup from EEPROM
 */
void RouterSetup::readFromEEPROM() {
	clear();
	eeprom.begin(ROUTER_NAMESPACE, false);
	setSpeed(eeprom.getFloat(SPEED, 1.0));
	setAcceleration(eeprom.getFloat(ACCELERATION, 1.0));
	setStepsPerRev(eeprom.getUInt(STEPSPERREV, 1600));
	setPitch(eeprom.getFloat(PITCH, 3.0));
	setToolLenghtSensorHeight(eeprom.getFloat(TOOLLENGHTSNHT, 10.0));
	setEncoderSpeedSlow(eeprom.getInt(ENCSPEEDSLOW, 1));
	setEncoderSpeedFast(eeprom.getInt(ENCSPEEDFAST, 20));
	setLevelHeightDiving(eeprom.getFloat(LVLHEIGHTDIVE, 3.0));
	setToolChangOnPowerOn(eeprom.getBool(TOOLCHGONPWRON, true));
	eeprom.end();
}

/**
 *  Writes the RouterSetup to EEPROM
 */
void RouterSetup::saveToEEPROM() {
	eeprom.begin(ROUTER_NAMESPACE, false);
	eeprom.putFloat(SPEED, getSpeed());
	eeprom.putFloat(ACCELERATION, getAcceleration());
	eeprom.putUInt(STEPSPERREV, getStepsPerRev());
	eeprom.putFloat(PITCH, getPitch());
	eeprom.putFloat(TOOLLENGHTSNHT, getToolLenghtSensorHeight());
	eeprom.putInt(ENCSPEEDSLOW, getEncoderSpeedSlow());
	eeprom.putInt(ENCSPEEDFAST, getEncoderSpeedFast());
	eeprom.putFloat(LVLHEIGHTDIVE, getLevelHeightDiving());
	eeprom.putBool(TOOLCHGONPWRON, isToolChangOnPowerOn());
	eeprom.end();
}

//void RouterSetup::printValues() {
//	Serial.print(String(SPEED) + ": " + String(getSpeed(), 1) + ", ");
//	Serial.print(String(ACCELERATION) + ": " + String(getAcceleration(), 1) + ", ");
//	Serial.print(String(STEPSPERREV) + ": " + String(getStepsPerRev()) + ", ");
//	Serial.print(String(PITCH) + ": " + String(getPitch(), 1) + ", ");
//	Serial.print(String(TOOLLENGHTSNHT) + ": " + String(getToolLenghtSensorHeight(), 2) + ", ");
//	Serial.print(String(ENCSPEEDSLOW) + ": " + String(getEncoderSpeedSlow()) + ", ");
//	Serial.print(String(ENCSPEEDFAST) + ": " + String(getEncoderSpeedFast()) + ", ");
//	Serial.print(String(LVLHEIGHTDIVE) + ": " + String(getLevelHeightDiving(), 2) + ", ");
//	Serial.println(String(TOOLCHGONPWRON) + ": " + String(isToolChangOnPowerOn()));
//}

void RouterSetup::initialize() {
	if (doInitialization) {
		configStepIndex = 0;
		String stepTxt = ConfigStep[configStepIndex];
		String option = getCfgOptForStepIndex(configStepIndex);
		option.trim();
		display.setConfigText(stepTxt);
		display.setConfigOption(option);
		doInitialization = false;
	}
}

void RouterSetup::onRotaryControlerSwitch() {
	configStepIndex++;
	configStepIndex = configStepIndex % 9;
	String stepTxt = ConfigStep[configStepIndex];
	String option = getCfgOptForStepIndex(configStepIndex);
	option.trim();
	display.setConfigText(stepTxt);
	display.setConfigOption(option);
}

void RouterSetup::onRotaryControlerLongSwitch() {
	save();
	doInitialization = true;
}

void RouterSetup::save() {
	saveToEEPROM();
}

void RouterSetup::cancel() {
	doInitialization = true;
}


void RouterSetup::onRotaryControlerTurn(RotaryEncoder::Direction turn) {

	if (turn != RotaryEncoder::Direction::NOROTATION) {

		String value = "";
		int index = 0;
		int sign = (turn == RotaryEncoder::Direction::CLOCKWISE) ? 1 : -1;
		String unit = getCfgOptUnitForStepIndex(configStepIndex);

		switch (configStepIndex) {
			case 0:
				setSpeed(max(0.0, getSpeed() + 0.1 * sign));
				value = String(getSpeed(), 1);
				break;
			case 1:
				setAcceleration(max(0.0, getAcceleration() + 0.1 * sign));
				value = String(getAcceleration(), 1);
				break;
			case 2:
				index = getIndexOfStepsPerRevValue(getStepsPerRev());
				if (index > -1) {
					index = min(max((index + 1 * sign), 0), StepsPerRevOptsSize - 1) % 5;
					setStepsPerRev(StepsPerRevolutionOptions[index]);
					value = String(getStepsPerRev());
				}
				break;
			case 3:
				setPitch(max(0.0, getPitch() + 0.1 * sign));
				value = String(getPitch(), 1);
				break;
			case 4:
				setToolLenghtSensorHeight(max(0.0, getToolLenghtSensorHeight() + 0.01 * sign));
				value = String(getToolLenghtSensorHeight(), 2);
				break;
			case 5:
				setEncoderSpeedSlow(min(max(1, getEncoderSpeedSlow() + 1 * sign), 9));
				value = String(getEncoderSpeedSlow());
				break;
			case 6:
				setEncoderSpeedFast(min(max(10, getEncoderSpeedFast() + 1 * sign), 100));
				value = String(getEncoderSpeedFast());
				break;
			case 7:
				setLevelHeightDiving(min(max(0.1, getLevelHeightDiving() + 0.1 * sign), 10.0));
				value = String(getLevelHeightDiving(), 1);
				break;
			case 8:
				setToolChangOnPowerOn(sign > 0 ? true : false);
				value = isToolChangOnPowerOn() ? "JA" : "NEIN";
				break;
			default:
				break;
		}

		display.setConfigOption(value + " " + unit);
	}
}

//***************************************************************
//*********** Private methods ***********************************
//***************************************************************
String RouterSetup::getCfgOptForStepIndex(byte configStepIndex) {
	String value = "";
	String unit = getCfgOptUnitForStepIndex(configStepIndex);
	switch (configStepIndex) {
		case 0:
			value = String(getSpeed(), 1);
			break;
		case 1:
			value = String(getAcceleration(), 1);
			break;
		case 2:
			value = String(getStepsPerRev());
			break;
		case 3:
			value = String(getPitch(), 1);
			break;
		case 4:
			value = String(getToolLenghtSensorHeight(), 2);
			break;
		case 5:
			value = String(getEncoderSpeedSlow());
			break;
		case 6:
			value = String(getEncoderSpeedFast());
			break;
		case 7:
			value = String(getLevelHeightDiving(), 1);
			break;
		case 8:
			value = isToolChangOnPowerOn() ? "JA" : "NEIN";
			break;
		default:
			break;
	}
	return value + " " + unit;
}

String RouterSetup::getCfgOptUnitForStepIndex(byte configStepIndex) {
	String value = "";
	switch (configStepIndex) {
		case 0:
			value = "U/sec";
			break;
		case 1:
			value = "U/sec2";
			break;
		case 3:
			value = "mm";
			break;
		case 4:
			value = "mm";
			break;
		case 5:
			value = "mm/100";
			break;
		case 6:
			value = "mm/100";
			break;
		case 7:
			value = "mm";
			break;
		default:
			break;
	}
	return value;
}

int RouterSetup::getIndexOfStepsPerRevValue(uint16_t value) {
	for (int i = 0; i < sizeof(StepsPerRevolutionOptions); i++) {
		if (StepsPerRevolutionOptions[i] == value) {
			return i;
		}
	}
	return -1;
}