/*
 * RouterElevator.cpp
 *
 *  Created on: 12.02.2022
 *      Author: FSmilari
 */

#include "RouterElevator.h"

RouterElevator::RouterElevator(ESP_FlexyStepper &_Stepper, Display &_display, RouterSetup &_Router_Setup,
		WLS &_WlsDetect, WLS &_Wls, int _LimitSwitch, int _DOWNWARD_DIR)
: Stepper(_Stepper), display(_display), Router_Setup(_Router_Setup), WlsDetect(_WlsDetect), Wls(_Wls), mode(FAST) {
	LimitSwitch = _LimitSwitch;
	DOWNWARD_DIR = _DOWNWARD_DIR;
	UPWARD_DIR = -DOWNWARD_DIR;
	targetDistance = 0.0;
	maxDiveDistance = 0.0;
	doDiveInitialization = true;
	nullingDone = false;
}

void RouterElevator::setZeroPosition() {
	Stepper.setCurrentPositionAsHomeAndStop();
	Stepper.setCurrentPositionInMillimeters(0);
	Stepper.setTargetPositionRelativeInMillimeters(0);
	targetDistance = 0.0;
	nullingDone = true;
	setMode(FAST);
	display.setDistanceValue(targetDistance);
}

void RouterElevator::moveRelativeInMillimeters(float distanceInMillimeters) {
	Stepper.setTargetPositionRelativeInMillimeters(distanceInMillimeters);
}

void RouterElevator::moveToLowerLimitSwitch(void) {
	Stepper.setTargetPositionInMillimeters(300 * DOWNWARD_DIR);
}

void RouterElevator::moveToUpperLimitSwitch(void) {
	Stepper.setTargetPositionInMillimeters(300 * UPWARD_DIR);
}

void RouterElevator::clearLimitSwitch(void) {
	Stepper.clearLimitSwitchActive();
}

void RouterElevator::tryReleaseLimitSwitch(void) {
	Stepper.moveRelativeInMillimeters(0.05 * previousDirection * -1); // move in opposite direction (away from switch)
}

bool RouterElevator::isLimitSwitchTriggerd() {
	return limitSwitchState == LOW;
}

bool RouterElevator::isTargetPositionReached() {
	return Stepper.getDistanceToTargetSigned() == 0;
}

void RouterElevator::moveToTarget(void) {
	Stepper.setTargetPositionInMillimeters(targetDistance);
}

void RouterElevator::moveToParkPosition(void) {
	float parkOffset = -5; //Router_Setup.getParkPostionOffset() * -1;
	targetDistance = parkOffset;
	Stepper.setTargetPositionInMillimeters(targetDistance);
	display.setDistanceValue(targetDistance);
}

bool RouterElevator::isWLSTriggerd() {
	if (WlsDetect.isConnected()) {
		if (Wls.isPlugged()) {
//			Serial.println("The Tool is away from WLS");
			Stepper.clearLimitSwitchActive();
			return false;
		} else if (Wls.isUnplugged()) {
//			Serial.println("The Tool touched the WLS");
			Stepper.setLimitSwitchActive(Stepper.LIMIT_SWITCH_COMBINED_BEGIN_AND_END);
			return true;
		}
	}
	return false;
}

void RouterElevator::limitSwitchHandler() {
	limitSwitchState = digitalRead(LimitSwitch);
	if (limitSwitchState == LOW) {
		// this will cause to stop any motion that is currently going on and block further movement in the same direction as long as the switch is active
		Stepper.setLimitSwitchActive(Stepper.LIMIT_SWITCH_COMBINED_BEGIN_AND_END);
	} else {
		// clear the limit switch flag to allow movement in both directions again
		clearLimitSwitch();
	}
}

void RouterElevator::readLimitSwitch() {
	limitSwitchState = digitalRead(LimitSwitch);
}

void RouterElevator::checkDirection() {
	previousDirection = Stepper.getDirectionOfMotion();
}

ValueMode RouterElevator::getMode() const {
	return mode;
}

void RouterElevator::setMode(ValueMode mode) {
	this->mode = mode;
	display.setMode(this->mode);
}

void RouterElevator::toggleMode() {
	setMode((mode.getValueMode() == SLOW) ? ValueMode(FAST) : ValueMode(SLOW));
}

void RouterElevator::onRotaryControlerTurn(RotaryEncoder::Direction turn) {
	if (turn != RotaryEncoder::Direction::NOROTATION) {
		int sign = (turn == RotaryEncoder::Direction::CLOCKWISE) ? 1 : -1;
		float increment = mode.isSlow() ? Router_Setup.getEncoderSpeedSlow() : Router_Setup.getEncoderSpeedFast();
		if (nullingDone) {
			targetDistance = max(0.0f, targetDistance + increment / 100 * sign);
		} else {
			targetDistance = targetDistance + increment / 100 * sign;
		}
		display.setDistanceValue(targetDistance);
		display.setDistanceUnit("mm");
	}
}

bool RouterElevator::isDoDiveInitialization() const {
	return doDiveInitialization;
}

void RouterElevator::setDoDiveInitialization(bool doDiveInitialization) {
	this->doDiveInitialization = doDiveInitialization;
}

float RouterElevator::getMaxDiveDistance() const {
	return maxDiveDistance;
}

void RouterElevator::setMaxDiveDistance() {
	if (isDoDiveInitialization()) {
		maxDiveDistance = targetDistance;
		targetDistance = 0.0;
		display.setDistanceValue(targetDistance);
		display.setDistanceUnit("mm");
		display.setMaxDiveDistance(maxDiveDistance);
		display.setLevelHeightDiving(Router_Setup.getLevelHeightDiving());
		display.setRefreshScreen();
		setDoDiveInitialization(false);
	}
}

bool RouterElevator::incrementDiveDistance() {
	float increment = Router_Setup.getLevelHeightDiving();
	targetDistance = min(maxDiveDistance, targetDistance + increment);
	display.setMaxDiveDistance(maxDiveDistance);
	display.setDistanceValue(targetDistance);
	display.setLevelHeightDiving(Router_Setup.getLevelHeightDiving());
	return targetDistance == maxDiveDistance;
}

bool RouterElevator::maxDiveDistanceReached() {
	return targetDistance == maxDiveDistance;
}


void RouterElevator::resetNullingDone() {
	this->nullingDone = false;
}