/*
 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 <Wire.h>
#include <WString.h>

#include "Display.h"
#include "ExEzButton.h"
#include "RotaryControler.h"
#include "RouterElevator.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 int MOVE_DOWNWARD = -1; // motor rotation counter clock wise
//static const int MOVE_UPWARD = 1;  // motor rotation clock wise

int intermediateState = 0;

ExEzButton RedButton(RedBtn_Pin, false, 2000);
ExEzButton GreenButton(GreenBtn_Pin, false, 2000);
ExEzButton BlueButton(BlueBtn_Pin, false, 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(Display);
RouterElevator Router_Elevator(Stepper, Display, Router_Setup, WlsDetect, Wls, LIMIT_SWITCH, MOVE_DOWNWARD);

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

//********************* Rotary test **************

long TimeOfLastDebounce = 0;
long DelayofDebounce = 5;

// Store previous Pins state
int PreviousCLK;
int PreviousDATA;

int displaycounter = 0; // Store current counter value
volatile boolean TurnDetected;


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

//**********************************
//*** Limit switch interrupt routine
//**********************************
void limitSwitchHandler() {
	Router_Elevator.limitSwitchHandler();
}

//*****************************************************************************************************
//*** 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());
	if (!Stepper.isStartedAsService()) {
		Stepper.startAsService(0);
	}

	if (Router_Setup.isToolChangOnPowerOn()) {
		SetActualStatus(TOOL_CHANGE);
	} else {
		SetActualStatus(IDLE);
	}
}

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

	SetActualStatus(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_PULLUP);
//	pinMode(RotEnc_Dta_Pin, INPUT_PULLUP);
	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.setRefreshScreen();
	Display.showBrand();
	delay(1500);
	Display.setRefreshScreen();
	Initialize();
}

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

	Display.setWlsConnected(WlsDetect.isConnected());
	Display.showFrame(actualStatus);

	switch (actualStatus) {

		case INITIALIZATION:
			Initialize();
			break;

		case TOOL_CHANGE:
			printStatus("TOOL_CHANGE");
			originStatus = TOOL_CHANGE;
			if (intermediateState == 0) {
				Router_Elevator.moveToUpperLimitSwitch();
				SetActualStatus(MOVING_ELEVATOR);
				intermediateState = 1;
			} else if (intermediateState == 1) {
				Router_Elevator.setZeroPosition();
				Router_Elevator.resetNullingDone();
				if (RotaryControler.isSwitchLongPressed()) {
					SetActualStatus(CONFIGURATION);
					intermediateState = 0;
				} else if (BlueButton.isPressed()) {
					SetActualStatus(IDLE);
					intermediateState = 0;
				}
			}
			break;

		case CONFIGURATION:
			printStatus("CONFIGURATION");
			Router_Setup.initialize();
			Router_Setup.onRotaryControlerTurn(RotaryControler.getEncoderMove());
			if (RotaryControler.isSwitchPressed()) {
				Router_Setup.onRotaryControlerSwitch();
			} else if (RotaryControler.isSwitchLongPressed()) {
				Router_Setup.onRotaryControlerLongSwitch();
				SetActualStatus(INITIALIZATION);
			} else if (BlueButton.isPressed()) {
				Router_Setup.cancel();
				SetActualStatus(IDLE);
			}
			break;

		case NULLING:
			printStatus("NULLING");
			if (RedButton.isPressed()) {
				Router_Elevator.setZeroPosition();
				SetActualStatus(IDLE);
			}
			break;

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

		case IDLE:
			printStatus("IDLE");
			originStatus = IDLE;
			if (RotaryControler.isSwitchPressed()) {
				Router_Elevator.toggleMode();
			}
			Router_Elevator.onRotaryControlerTurn(RotaryControler.getEncoderMove());
			if (RedButton.isLongPressed()) {
				if (WlsDetect.isConnected()) {
					SetActualStatus(NULLING_TLS);
				} else {
					SetActualStatus(NULLING);
				}
			} else if (BlueButton.isLongPressed()) {
				SetActualStatus(TOOL_CHANGE);
			} else if (GreenButton.isLongPressed()) {
				SetActualStatus(DIVING);
			} else if (GreenButton.isPressed()) {
				Router_Elevator.moveToTarget();
				SetActualStatus(MOVING_ELEVATOR);
			}
			break;

		case DIVING:
			printStatus("DIVING");
			originStatus = DIVING;
			Router_Elevator.setMaxDiveDistance();
			if (GreenButton.isPressed()) {
				Serial.println("GB && BB pressing");
				if (!Router_Elevator.maxDiveDistanceReached()) {
					Router_Elevator.incrementDiveDistance();
					Router_Elevator.moveToTarget();
					SetActualStatus(MOVING_ELEVATOR);
				} else {
					originStatus = IDLE;
					Router_Elevator.setDoDiveInitialization(true);
					Router_Elevator.moveToParkPosition();
					SetActualStatus(MOVING_ELEVATOR);
				}
			} else if (GreenButton.isLongPressed()) {
				originStatus = IDLE;
				Router_Elevator.setDoDiveInitialization(true);
				Router_Elevator.moveToParkPosition();
				SetActualStatus(MOVING_ELEVATOR);
			}
			break;

		case MOVING_ELEVATOR:
			printStatus("MOVING_ELEVATOR");
			if (Router_Elevator.isLimitSwitchTriggerd()) {
				delay(200);
				SetActualStatus(RELEASE_SWITCH);
			} else {  // limitSwitchState is HIGH
				if (Router_Elevator.isTargetPositionReached()) {
					SetActualStatus(originStatus);
					delay(200);
				} else if (Router_Elevator.isWLSTriggerd()) {
					SetActualStatus(originStatus);
					delay(200);
				}
				Router_Elevator.clearLimitSwitch();
				Router_Elevator.checkDirection();
			}
			break;

		case RELEASE_SWITCH:
			printStatus("RELEASE_SWITCH");
			if (Router_Elevator.isLimitSwitchTriggerd()) {
				Router_Elevator.tryReleaseLimitSwitch();
			} else {
				SetActualStatus(originStatus);
			}
			break;

		default:
			break;
	}
}

void SetActualStatus(Status newStatus) {
	if (actualStatus != newStatus) {
		actualStatus = newStatus;
		Display.setRefreshScreen();
	}
}