/*
 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>

#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define SCREEN_ADDRESS 0x3C

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;

ExEzButton RedButton(RedBtn_Pin, false, 2000);
ExEzButton GreenButton(GreenBtn_Pin);
ExEzButton BlueButton(BlueBtn_Pin);
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 22, SDA on PIN 23


//**********************
//*** SETUP ************
//**********************
void setup() {
	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);

	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

	// Clear the buffer
	Display.clearDisplay();

	testdrawchar();
	delay(5000); // Pause for 5 seconds
	Display.clearDisplay();

	testdrawstyles();
}

//**********************
//*** 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);
	}

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

}

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.setTextSize(1);             // Normal 1:1 pixel scale
	Display.setTextColor(SSD1306_WHITE);        // Draw white text
	Display.setCursor(0, 0);             // Start at top-left corner
	Display.println(F("Hello, world!"));

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