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

#include "RouterElevator.h"

RouterElevator::RouterElevator(ESP_FlexyStepper &_Stepper, Display &_display, WLS &_WlsDetect, WLS &_Wls, int _LimitSwitch, int _DOWNWARD_DIR) :
		Stepper(_Stepper), display(_display), WlsDetect(_WlsDetect), Wls(_Wls) {
	LimitSwitch = _LimitSwitch;
	DOWNWARD_DIR = _DOWNWARD_DIR;
	UPWARD_DIR = -DOWNWARD_DIR;
}

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

void RouterElevator::moveRelativeInMillimeters(float distanceInMillimeters) {
	Stepper.setTargetPositionInMillimeters(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;
}

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