#include <Keypad.h>
#include <LiquidCrystal.h>

// LCD
LiquidCrystal lcd(31, 33, 35, 37, 39, 41);

const byte ROWS = 4;
const byte COLS = 4;
float Zstep;
float XYstep;
unsigned int Feedrate;
unsigned int SpindleSpeed = 10000;
const unsigned int message_delay = 750;	// milliseconds for message to stay up
byte spindle = 0;
float all_step_sizes[] = { 0.1, 0.2, 0.5, 1.0, 2.0, 5.0, 10.0, 20.0, 50.0, 100.0, 200.0 };
int all_feed_rates[] = { 1, 10, 100, 200, 500, 1000, 2000, 5000 };
#define MAX_XY_STEP_SIZES (sizeof(all_step_sizes)/sizeof(float))
#define MAX_Z_STEP_SIZES 8
#define MAX_FEED_RATES (sizeof(all_feed_rates)/sizeof(int))
unsigned int zstep_idx;
unsigned int xystep_idx;
unsigned int feedrate_idx;
char *message;
long message_timer;

#define DBG 0

char hexaKeys[ROWS][COLS] = {
  {'1', '2', '3', 'A'},
  {'4', '5', '6', 'B'},
  {'7', '8', '9', 'C'},
  {'*', '0', '#', 'D'}
};

byte rowPins[ROWS] = {9, 8, 7, 6};
byte colPins[COLS] = {5, 4, 3, 2};

Keypad customKeypad = Keypad(makeKeymap(hexaKeys), rowPins, colPins, ROWS, COLS);

void printf_N_Pf (byte total_places, byte decimal_places, float fv) {
byte whole_places = total_places - ((decimal_places) ? (decimal_places + 1) : 0);
unsigned int power = 1;
    for(byte i = 1 ; i < whole_places; i++) { power *= 10; }
    for( ; power > 1 ; power /= 10) {
	if(fv < power) { lcd.print(F(" ")); }
    }
    lcd.print(fv, decimal_places);
}

void update_lcd (void) {
    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print(F("XY Step = "));
    printf_N_Pf(5, 1, XYstep);
    lcd.setCursor(0, 1);
    lcd.print(F(" Z Step = "));
    printf_N_Pf(5, 1, Zstep);
    lcd.setCursor(0, 2);
    lcd.print(F("Feedrate = "));
    printf_N_Pf(4, 0, Feedrate);
    if(message) {
	lcd.setCursor(0, 3);
	lcd.print(message);
	message_timer = millis();
    }
}

void clear_message (void) {
    if(message_timer + message_delay < millis()) {
	lcd.setCursor(0, 3);
	lcd.print(F("                    "));
	message = NULL;
    }
}

void spindle_control (void) {
    if(spindle) {
	Serial.print(F("M3 S"));
	Serial.println(SpindleSpeed);
    }
    else {
	Serial.println(F("M5"));
    }
}

void setup() {
    Serial.begin(115200);
#if DBG
    Serial.println(F("\nStarting GRBL Offline controller"));
#endif
    spindle_control();
    zstep_idx = 3;
    xystep_idx = 6;
    feedrate_idx = 5;
    XYstep = all_step_sizes[xystep_idx];
    Zstep = all_step_sizes[zstep_idx];
    Feedrate = all_feed_rates[feedrate_idx];
    update_lcd();
    // initialize the machine
    Serial.print(F("$30="));	// tell GRBL what max spindle speed is
    Serial.println(SpindleSpeed);
    delay(50);
    Serial.write(153);	// 0x91 reset spindle, set to 100%
}

void loop() {
  char customKey = customKeypad.getKey();
  clear_message();
  if (customKey) {
#if DBG
    Serial.print("key = ");
    Serial.println(customKey);
#endif

    switch(customKey) {
	case '0' : // XY - 1
	    spindle ^= 1;
	    spindle_control();
	    message = (spindle) ? "spindle on" : "spindle off";
	    break;
	case '1' :
	    Serial.print(F("$J=G91 Z"));
	    Serial.print(Zstep);
	    Serial.print(F("F"));
	    Serial.println(Feedrate);
	    message = "Move Z+";
	    //lcd.print(XYstep);
	    break;
	case '2' :
	    Serial.print(F("$J=G91 Y"));
	    Serial.print(XYstep);
	    Serial.print(F("F"));
	    Serial.println(Feedrate);
	    message = "Move Y+";
	    break;
	case '3' : // Feedrate up
	    if(feedrate_idx < MAX_FEED_RATES - 1) {
		Feedrate = all_feed_rates[++feedrate_idx];
	    }
	    else {
		message = "Feed rate at maximum";
	    }
	    break;
	case '4' :
	    Serial.print(F("$J=G91 X"));
	    Serial.print (-XYstep);
	    Serial.print ("F");
	    Serial.println (Feedrate);
	    message = "Move X-";
	    break;
	case '5' : // Home
	    Serial.println(F("$H"));
	    message = "Home";
	    break;
	case '6' :
	    Serial.print(F("$J=G91 X"));
	    Serial.print (XYstep);
	    Serial.print ("F");
	    Serial.println (Feedrate);
	    message = "Move X+";
	    break;
	case '7' :
	    Serial.print(F("$J=G91 Z"));
	    Serial.print(-Zstep);
	    Serial.print(F("F"));
	    Serial.println(Feedrate);
	    message = "Move Z-";
	    break;
	case '8' :
	    Serial.print(F("$J=G91 Y"));
	    Serial.print(-XYstep);
	    Serial.print(F("F"));
	    Serial.println(Feedrate);
	    message = "Move Y-";
	    break;
	case '9' : // Feedrate down
	    if(feedrate_idx) {
		Feedrate = all_feed_rates[--feedrate_idx];
	    }
	    else {
		message = "Feed rate at minimum";
	    }
	    //Serial.println(Zstep);
	    break;
	case 'A' : // Button A + XY step
	    if(xystep_idx < MAX_XY_STEP_SIZES - 1) {
		XYstep = all_step_sizes[++xystep_idx];
	    }
	    else {
		message = "XY step at maximum.";
	    }
	    //Serial.println(XYstep);
	    break;
	case 'B' : // Button B -xy step
	    if(xystep_idx) {
		XYstep = all_step_sizes[--xystep_idx];
	    }
	    else {
		message = "XY step at minimum.";
	    }
	    //Serial.println(XYstep);
	    break;
	case 'C' : // Button C +Z step
	    if(zstep_idx < MAX_Z_STEP_SIZES - 1) {
		Zstep = all_step_sizes[++zstep_idx];
	    }
	    else {
		message = "Z step at maximum.";
	    }
	    //Serial.println(Zstep);
	    break;
	case 'D' : // Button D -Z step
	    if(zstep_idx) {
		Zstep = all_step_sizes[--zstep_idx];
	    }
	    else {
		message = "Z step at minimum.";
	    }
	    //Serial.println(Zstep);
	    break;
	case '*' :
	    Serial.println("!");
	    break;
	case '#' :
	    Serial.println("~");
	    break;
    }
    update_lcd();
  }
}