/*
 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 <RotaryEncoder.h>
#include "WLS.h"
#include "ExEzButton.h"
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <ESP_FlexyStepper.h>
#include "fonts/titillium_web_6pt7b.h"
#include "fonts/titillium_web_8pt7b.h"
#include "fonts/titillium_web_12pt7b.h"
#include "fonts/titillium_web_24pt7b.h"
#include "fonts/titillium_web_30pt7b.h"

#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define SCREEN_ADDRESS 0x3C
#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;

// Speed settings
const int DISTANCE_TO_TRAVEL_IN_STEPS = 2000;
const int SPEED_IN_STEPS_PER_SECOND = 300;
const int ACCELERATION_IN_STEPS_PER_SECOND = 800;
const int DECELERATION_IN_STEPS_PER_SECOND = 800;

//variables for software debouncing of the limit switches
unsigned long lastDebounceTime = 0;
unsigned long debounceDelay = 100; //the minimum delay in milliseconds to check for bouncing of the switch. Increase this slighlty if you switches tend to bounce a lot
byte limitSwitchState = 1;
byte oldConfirmedLimitSwitchState = 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);

RotaryEncoder encoder(RotEnc_Dta_Pin, RotEnc_Clk_Pin, RotaryEncoder::LatchMode::FOUR3);

Adafruit_SSD1306 Display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1); // SCK on Pin 23, SDA on PIN 22

ESP_FlexyStepper stepper;

int previousDirection = 1;


void limitSwitchHandler()
{
	limitSwitchState = digitalRead(LIMIT_SWITCH);
	lastDebounceTime = millis();
}


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

	Wire.begin(SDA, SCX);

	Serial.begin(115200);
	if (!Display.begin(SSD1306_SWITCHCAPVCC, SCREEN_ADDRESS, true, true)) {
		Serial.println(F("SSD1306 allocation failed"));
		for (;;)
			; // Don't proceed, loop forever
	}

	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);
	RotarySwitch.setDebounceTime(50);

	Display.display();
	delay(2000); // Pause for 2 seconds

//	testdrawchar();
//	delay(5000); // Pause for 5 seconds

	testdrawstyles();

	//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(SPEED_IN_STEPS_PER_SECOND);
//	stepper.setAccelerationInStepsPerSecondPerSecond(ACCELERATION_IN_STEPS_PER_SECOND);
//	stepper.setDecelerationInStepsPerSecondPerSecond(DECELERATION_IN_STEPS_PER_SECOND);
//
//	stepper.startAsService(1);
}

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

	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;
	encoder.tick();
	int newPos = encoder.getPosition();
	if (pos != newPos) {
		Serial.print("pos:");
		Serial.print(newPos);
		Serial.print(" dir:");
		Serial.println((int) (encoder.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) {
	Display.clearDisplay();

	Display.setTextSize(1);      // Normal 1:1 pixel scale
	Display.setTextColor(SSD1306_WHITE); // Draw white text
	Display.setCursor(0, 0);     // Start at top-left corner
	Display.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')
			Display.write(' ');
		else
			Display.write(i);
	}

	Display.display();
	delay(2000);
}

void testdrawstyles(void) {
	Display.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++) {
		Display.clearDisplay();
		Display.setFont(&titillium_web_semibold30pt7b);
		Display.setTextSize(1);             // Normal 1:1 pixel scale
		Display.setTextColor(SSD1306_WHITE);        // Draw white text
		sprintf(decimals, "%02d", i);
		if (i < 33) {
			calculateWH("99.", w, h);
			Display.setCursor((SCREEN_WIDTH / 2 - 1) - w, SCREEN_HEIGHT / 2 + h / 2);
			Display.print("99.");
		} else if (i >= 33 && i < 66) {
			calculateWH("01.", w, h);
			Display.setCursor((SCREEN_WIDTH / 2 - 1) - w, SCREEN_HEIGHT / 2 + h / 2);
			Display.print("01.");
		} else {
			calculateWH("77.", w, h);
			Display.setCursor((SCREEN_WIDTH / 2 - 1) - w, SCREEN_HEIGHT / 2 + h / 2);
			Display.print("77.");
		}
		Display.println(decimals);
		Display.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);
//
	Display.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;

	Display.getTextBounds(units, x, y, &x1, &y1, &w1, &h1);
	w = w1;
	h = h1;
}