/*
 * 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(SLOW) {
	LimitSwitch = _LimitSwitch;
	DOWNWARD_DIR = _DOWNWARD_DIR;
	UPWARD_DIR = -DOWNWARD_DIR;
	targetDistance = 0;
}

void RouterElevator::setZeroPosition() {
	Stepper.setCurrentPositionAsHomeAndStop();
	Stepper.setCurrentPositionInMillimeters(0);
	Stepper.setTargetPositionRelativeInMillimeters(0);
}

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

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

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::checkDirection() {
	previousDirection = Stepper.getDirectionOfMotion();
}

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

void RouterElevator::setMode(ValueMode mode) {
	this->mode = mode;
	Serial.println("RouterElevator::setMode: " + this->mode.toString());
	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;
//		//TODO: Update the targetDistance and show it on display
//	}
}