/*
 Steuerung für Frästisch_SFTools

 Diese Datei enthält den Code für die Implementierung der Steuerung eines Frästisches mit 2 Nema17 Schrittmotoren.
 Sie unterstützt:
 - Schnelles/langsames Verstellen der Fräserhöhe
 - Werkzeugwechsel
 - Automatisches Nullen mit WLS
 - Eintauchen mit vordefinierter Tiefe
 - Für obige Funktionen notwendige Konfiguration

 Erstellt:	05.01.2021
 Autor: 	Flo Smilari
 */

#include <Arduino.h>
#include <esp32-hal.h>
#include <esp32-hal-gpio.h>
#include <ESP_FlexyStepper.h>
#include <HardwareSerial.h>
#include <pins_arduino.h>
#include <RotaryEncoder.h>
#include <Wire.h>
#include <WString.h>

#include "Display.h"
#include "ExEzButton.h"
#include "RotaryControler.h"
#include "RouterSetup.h"
#include "Status.h"
#include "WLS.h"

#define SDA 22
#define SCX 23
#define STEP 2
#define DIR 4
#define LIMIT_SWITCH 5

static const byte led_gpio = 15;

static const int WLS_Pin = 34;
static const int WLS_DETECT_Pin = 35;

static const int GreenBtn_Pin = 12;
static const int RedBtn_Pin = 13;
static const int BlueBtn_Pin = 14;

static const int RotEnc_Switch_Pin = 25;
static const int RotEnc_Clk_Pin = 32;
static const int RotEnc_Dta_Pin = 33;

static const float MOVE_DOWNWARD = -1; // motor rotation counter clock wise
static const float MOVE_UPWARD = 1;    // motor rotation clock wise

byte limitSwitchState = 1;
int previousDirection = 0;
int nullingTLS_intermediateState = 0;

ExEzButton RedButton(RedBtn_Pin, false, 2000);
ExEzButton GreenButton(GreenBtn_Pin, false, 2000);
ExEzButton BlueButton(BlueBtn_Pin, false, 2000);
//ExEzButton RotarySwitch(RotEnc_Switch_Pin, true, 2000);

WLS WlsDetect(WLS_DETECT_Pin, true);
WLS Wls(WLS_Pin);

RotaryControler RotaryControler(RotEnc_Dta_Pin, RotEnc_Clk_Pin, RotEnc_Switch_Pin);
Display Display;
ESP_FlexyStepper Stepper;
RouterSetup Router_Setup;

Status actualStatus;
Status originStatus;
String oldStatus = "";

void printStatus(String actualStatus) {
	if (!oldStatus.equals(actualStatus)) {
		Serial.println(actualStatus);
		oldStatus = actualStatus;
	}
}

//**********************************
//*** Limit switch interrupt routine
//**********************************
void limitSwitchHandler() {
	limitSwitchState = digitalRead(LIMIT_SWITCH);
}

//*****************************************************************************************************
//*** Initialization routine. Reads the eeprom first and sets the (potentially new) configured values.
//*****************************************************************************************************
void Initialize() {
	Router_Setup.readFromEEPROM();
	Router_Setup.printValues();

	Display.showInitialization();
	//attach an interrupt to the IO pin of the limit switch and specify the handler function
	attachInterrupt(digitalPinToInterrupt(LIMIT_SWITCH), limitSwitchHandler, CHANGE);
	Stepper.connectToPins(STEP, DIR);
	// set the speed and acceleration rates for the stepper motor
	Stepper.setSpeedInStepsPerSecond(Router_Setup.getSpeed() * Router_Setup.getStepsPerRev());
	Stepper.setStepsPerRevolution(Router_Setup.getStepsPerRev());
	Stepper.setStepsPerMillimeter(Router_Setup.getStepsPerRev() / Router_Setup.getPitch());
	Stepper.setAccelerationInStepsPerSecondPerSecond(Router_Setup.getAcceleration() * Router_Setup.getStepsPerRev());
	Stepper.setDecelerationInStepsPerSecondPerSecond(Router_Setup.getAcceleration() * Router_Setup.getStepsPerRev());
	Stepper.startAsService(0);
	if (Router_Setup.isToolChangOnPowerOn()) {
		actualStatus = TOOL_CHANGE;
	} else {
		actualStatus = IDLE;
	}
}

//**********************
//*** SETUP ************
//**********************
void setup() {

	actualStatus = INITIALIZATION;

	Wire.begin(SDA, SCX);

	Serial.begin(115200);
	Display.init();
	Display.display();

	pinMode(led_gpio, OUTPUT);
	pinMode(GreenBtn_Pin, INPUT);
	pinMode(RedBtn_Pin, INPUT);
	pinMode(BlueBtn_Pin, INPUT);
	pinMode(RotEnc_Switch_Pin, INPUT);
	pinMode(RotEnc_Clk_Pin, INPUT);
	pinMode(RotEnc_Dta_Pin, INPUT);
	pinMode(LIMIT_SWITCH, INPUT);

	RedButton.setDebounceTime(50); // set debounce time to 50 millis
	GreenButton.setDebounceTime(50);
	BlueButton.setDebounceTime(50);
	WlsDetect.setDebounceTime(50);
	Wls.setDebounceTime(50);
	RotaryControler.setDebounceTime(50);

	delay(1500);
	Display.showBrand();
	delay(1500);
	Initialize();
}

//**********************
//*** MAIN LOOP ********
//**********************
void loop() {
	RedButton.loop(); // MUST call the loop() function first
	GreenButton.loop();
	BlueButton.loop();
	WlsDetect.loop();
	Wls.loop();
	RotaryControler.loop();

	Display.showFrame(actualStatus);

	switch (actualStatus) {

		case INITIALIZATION:
			Initialize();
			break;

		case TOOL_CHANGE:
			printStatus("TOOL_CHANGE");
			if (RotaryControler.isSwitchLongPressed()) {
				actualStatus = CONFIGURATION;
			} else if (BlueButton.isPressed()) {
				actualStatus = IDLE;
			}
			break;

		case CONFIGURATION:
			printStatus("CONFIGURATION");
			if (RotaryControler.isSwitchLongPressed()) {
				actualStatus = INITIALIZATION;
			} else if (BlueButton.isPressed()) {
				actualStatus = IDLE;
			}
			break;

		case NULLING:
			printStatus("NULLING");
			if (RedButton.isPressed()) {
				Stepper.setCurrentPositionInMillimeters(0);
				Stepper.setTargetPositionRelativeInMillimeters(0);
				actualStatus = IDLE;
			}
			break;

		case NULLING_TLS:
			originStatus = NULLING_TLS;
			if (nullingTLS_intermediateState == 0) {
				printStatus("NULLING_TLS,0");
				//Move elevator to lowest point (lower limit switch triggers)
				Stepper.setTargetPositionRelativeInMillimeters(300 * MOVE_DOWNWARD);
				actualStatus = MOVING_ELEVATOR;
				nullingTLS_intermediateState = 1;
			} else if (nullingTLS_intermediateState == 1) {
				printStatus("NULLING_TLS,1");
				if (RedButton.isPressed()) {
					//Move elevator until it touch the TLS (WLS_PIN goes HIGH)
					Stepper.setTargetPositionRelativeInMillimeters(300 * MOVE_UPWARD);
					actualStatus = MOVING_ELEVATOR;
					nullingTLS_intermediateState = 2;
				}
			} else if (nullingTLS_intermediateState == 2) {
				printStatus("NULLING_TLS,2");
				//Move elevator back about toolLenghtSensorHeight (will be the 0-Position)
				Stepper.clearLimitSwitchActive();
				Stepper.setTargetPositionRelativeInMillimeters(Router_Setup.getToolLenghtSensorHeight() * MOVE_DOWNWARD);
				Serial.println(String(Router_Setup.getToolLenghtSensorHeight() * MOVE_DOWNWARD, 2));
				actualStatus = MOVING_ELEVATOR;
				nullingTLS_intermediateState = 3;
			} else { // nullingTLS_intermediateState is 3
				printStatus("NULLING_TLS,3");
				//Set the 0-Position as actual position
				Stepper.setCurrentPositionInMillimeters(0);
				Stepper.setTargetPositionRelativeInMillimeters(0);
				actualStatus = IDLE;
				nullingTLS_intermediateState = 0;
			}
			break;

		case IDLE:
			printStatus("IDLE");
			if (RedButton.isLongPressed()) {
				if (WlsDetect.isConnected()) {
					actualStatus = NULLING_TLS;
				} else {
					actualStatus = NULLING;
				}
			} else if (BlueButton.isLongPressed()) {
				actualStatus = TOOL_CHANGE;
			}
			break;

		case DIVING:
			break;

		case MOVING_ELEVATOR:
			printStatus("MOVING_ELEVATOR");
			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);
				delay(200);
				actualStatus = RELEASE_SWITCH;
			} else {  // limitSwitchState is HIGH
				if (Stepper.getDistanceToTargetSigned() == 0) {
					actualStatus = originStatus;
					delay(200);
				} else if (WlsDetect.isConnected()) {
					if (Wls.isPlugged()) {
						Serial.println("The Tool is away from WLS");
						Stepper.clearLimitSwitchActive();
					} else if (Wls.isUnplugged()) {
						Serial.println("The Tool touched the WLS");
						Stepper.setLimitSwitchActive(Stepper.LIMIT_SWITCH_COMBINED_BEGIN_AND_END);
						actualStatus = originStatus;
						delay(200);
					}
				}
				Stepper.clearLimitSwitchActive();
				previousDirection = Stepper.getDirectionOfMotion();
			}
			break;

		case RELEASE_SWITCH:
			if (limitSwitchState == LOW) {
				Stepper.moveRelativeInMillimeters(0.05 * previousDirection * -1); // move in opposite direction (away from switch)
			} else {
				actualStatus = originStatus;
			}
			break;

		default:
			break;
	}

//*********************** OLD EXAMPLE AND EXPERIMENTAL CODE ************************************

//	if (WlsDetect.isPlugged()) {
//		Serial.println("The WLS was connected");
//	} else if (WlsDetect.isUnplugged()) {
//		Serial.println("The WLS was disconnected");
//	}
//
//	if (WlsDetect.getStateRaw() == HIGH) {
//		if (Wls.isPlugged()) {
//			Serial.println("The Tool is away from WLS");
//		} else if (Wls.isUnplugged()) {
//			Serial.println("The Tool touched the WLS");
//		}
//	}
//
//	if (RedButton.isPressed()) {
//		Serial.println("Red button was pressed");
//		digitalWrite(led_gpio, HIGH);
//		Serial.print("Limit Switch: ");
//		Serial.println(digitalRead(LIMIT_SWITCH) ? "HIGH" : "LOW");
//	}
//
//	if (RedButton.isLongPressed()) {
//		Serial.println("Red button was long pressed");
//		digitalWrite(led_gpio, LOW);
//	}
//
//	bool greenPressed = GreenButton.isPressed();
//	bool bluePressed = BlueButton.isPressed();
//
//	if (GreenButton.isPressing() && BlueButton.isPressing()) {
//		Serial.println("Blue and green button were simultaneous pressed");
//	} else if (greenPressed) {
//		Serial.println("Green button was pressed");
//	} else if (bluePressed) {
//		Serial.println("Blue button was pressed");
//	}
//
//	if (RotarySwitch.isPressed()) {
//		Serial.println("Rotary switch was pressed");
//	}
//	if (RotarySwitch.isLongPressed()) {
//		Serial.println("Rotary switch long was pressed");
//	}
//
//	static int pos = 0;
	//RotaryControler.tick();
	//int newPos = RotaryControler.getPosition();
//	if (pos != newPos) {
//		Serial.print("pos:");
//		Serial.print(newPos);
//		Serial.print(" dir:");
//	Serial.println((int) (RotaryControler.getDirection()));
//		pos = newPos;
//	}

//	if (limitSwitchState == LOW) {
//		stepper.setLimitSwitchActive(stepper.LIMIT_SWITCH_COMBINED_BEGIN_AND_END); // 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 agtive
//	} else {
//		stepper.clearLimitSwitchActive(); // clear the limit switch flag to allow movement in both directions again
//	}
//
//	if (limitSwitchState == HIGH && stepper.getDistanceToTargetSigned() == 0) {
//		delay(100);
//		previousDirection *= -1;
//		long relativeTargetPosition = DISTANCE_TO_TRAVEL_IN_STEPS * previousDirection;
//		Serial.printf("Moving stepper by %ld steps\n", relativeTargetPosition);
//		stepper.setTargetPositionRelativeInSteps(relativeTargetPosition);
//	}

}

//void testdrawchar(void) {
//	OLED.clearDisplay();
//
//	OLED.setTextSize(1);      // Normal 1:1 pixel scale
//	OLED.setTextColor(SSD1306_WHITE); // Draw white text
//	OLED.setCursor(0, 0);     // Start at top-left corner
//	OLED.cp437(true);         // Use full 256 char 'Code Page 437' font
//
//	// Not all the characters will fit on the Display. This is normal.
//	// Library will draw what it can and the rest will be clipped.
//	for (int16_t i = 0; i < 256; i++) {
//		if (i == '\n')
//			OLED.write(' ');
//		else
//			OLED.write(i);
//	}
//
//	OLED.display();
//	delay(2000);
//}
//
//void testdrawstyles(void) {
//	OLED.clearDisplay();

//	Display.setFont(&Rubik_Regular8pt7b);
//	Display.setTextSize(1);             // Normal 1:1 pixel scale
//	Display.setTextColor(SSD1306_WHITE);        // Draw white text
//	Display.setCursor(0, 8);             // Start at top-left corner
//	Display.println(F("Hello, world!"));
//
//	Display.setFont(&Rubik_Regular12pt7b);
//	Display.setTextSize(1);             // Normal 1:1 pixel scale
//	Display.setTextColor(SSD1306_WHITE);        // Draw white text
//	Display.setCursor(0, 24);             // Start at top-left corner
//	Display.println(F("Hello, world!"));

//	Display.drawRect(0, 15, 128, 13, SSD1306_WHITE);

//	Display.setFont(&Rubik_Regular24pt7b);
//	Display.setTextSize(1);             // Normal 1:1 pixel scale
//	Display.setTextColor(SSD1306_WHITE);        // Draw white text
//	Display.setCursor(0, 55);             // Start at top-left corner
//	Display.println(F("Hello, world!"));
//
//	Display.display();
//	delay(2000);
//
//	Display.clearDisplay();

//	char decimals[3];
//	uint16_t w = 0, h = 0;
//	for (int i = 0; i <= 99; i++) {
//		OLED.clearDisplay();
//		OLED.drawRect(0, 0, 128, 64, SSD1306_WHITE);
//		OLED.setFont(&titillium_web_semibold30pt7b);
//		OLED.setTextSize(1);             // Normal 1:1 pixel scale
//		OLED.setTextColor(SSD1306_WHITE);        // Draw white text
//		sprintf(decimals, "%02d", i);
//		if (i < 33) {
//			calculateWH("99.", w, h);
//			OLED.setCursor((SCREEN_WIDTH / 2 - 1) - w, SCREEN_HEIGHT / 2 + h / 2);
//			OLED.print("99.");
//		} else if (i >= 33 && i < 66) {
//			calculateWH("01.", w, h);
//			OLED.setCursor((SCREEN_WIDTH / 2 - 1) - w, SCREEN_HEIGHT / 2 + h / 2);
//			OLED.print("01.");
//		} else {
//			calculateWH("55.", w, h);
//			OLED.setCursor((SCREEN_WIDTH / 2 - 1) - w, SCREEN_HEIGHT / 2 + h / 2);
//			OLED.print("55.");
//		}
//		OLED.println(decimals);
//		OLED.display();
//		delay(100);
//	}

//	Display.setFont(&Rubik_Regular30pt7b);
//	Display.setTextSize(1);             // Normal 1:1 pixel scale
//	Display.setTextColor(SSD1306_WHITE);        // Draw white text
//	Display.setCursor(0, 55);             // Start at top-left corner
//	Display.println(F("99.99"));
//
//	Display.display();
//	delay(1000);
//
//	Display.clearDisplay();
//
//	Display.setFont(&Rubik_Regular30pt7b);
//	Display.setTextSize(1);             // Normal 1:1 pixel scale
//	Display.setTextColor(SSD1306_WHITE);        // Draw white text
//	Display.setCursor(0, 55);             // Start at top-left corner
//	Display.println(F("01.11"));

//	Display.setTextColor(SSD1306_BLACK, SSD1306_WHITE); // Draw 'inverse' text
//	Display.println("3.141592");
//
//	Display.setTextSize(2);             // Draw 2X-scale text
//	Display.setTextColor(SSD1306_WHITE);
//	Display.print(F("0x"));
//	Display.println(0xDEADBEEF, HEX);
//
//	OLED.display();
//	delay(2000);
//}

//void calculateWH(String units, uint16_t &w, uint16_t &h) {
//	int x = 0;
//	int y = 0;
//	int16_t x1, y1;
//	uint16_t w1, h1;
//
//	OLED.getTextBounds(units, x, y, &x1, &y1, &w1, &h1);
//	w = w1;
//	h = h1;
//}