#include "Keypad.h"

#include <SPI.h>
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>

#define SCREEN_WIDTH 128 // OLED display width, in pixels
#define SCREEN_HEIGHT 64 // OLED display height, in pixels

// Declaration for an SSD1306 display connected to I2C (SDA, SCL pins)

#define OLED_RESET     -1 // Reset pin # (or -1 if sharing Arduino reset pin)
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);


#define JOG 0
#define RUN 1
#define ZERO 2
byte status =0;//JOG
bool continue_move=false;
String gcodeString;
int previousMillis  = 0;
int currentMillis   = 0;

const byte ROWS = 4; //four rows
const byte COLS = 4; //four columns
static byte kpadState;

byte keys[ROWS][COLS] = {
  {9,1,10,5,},
  {3,16,4,6},
  {11,2,12,13},
  {7,8,14,15}
};
// pin uscita
byte rowPins[ROWS] = { 9, 8, 7, 6}; 
byte colPins[COLS] = { 5, 4, 3, 2}; 

Keypad keypad = Keypad(makeKeymap(keys), rowPins, colPins, ROWS, COLS);

void setup() {
  Serial.begin(9600);
//Serial.println("setup");
    // SSD1306_SWITCHCAPVCC = generate display voltage from 3.3V internally
   if(!display.begin(SSD1306_SWITCHCAPVCC, 0x3D)) { // Address 0x3D for 128x64
    Serial.println(F("SSD1306 allocation failed"));
    
   }

  // // Show initial display buffer contents on the screen --
  // // the library initializes this with an Adafruit splash screen.
  // display.display();
  // delay(2000); // Pause for 2 seconds

  // // Clear the buffer
  // display.clearDisplay();

  // // Draw a single pixel in white
  // display.drawPixel(10, 10, SSD1306_WHITE);

  // // Show the display buffer on the screen. You MUST call display() after
  // // drawing commands to make them visible on screen!
  // display.display();
  // delay(2000);
   keypad.begin( makeKeymap(keys) );
    
    keypad.addEventListener(keypadEvent_routine);  // Add an event listener.
    keypad.setHoldTime(300);                   // Default is 1000mS
    
}

//durante jog 1 singolo click sposta0.1 
//durante HOLD sposta 1 in continuo a scadenze in accordo con buffer grbl
//per permettere il moto continuo fino a released
String  axis[8] = {
  "Y","Y-","X","X-","Z","Z-","A","A-"
};
//$J=G21G91Z1F2000
void move_axis(byte key , float step_size){
    float feed=2000.0;//vedere poi di adeguare velocita e stepsize per mov continuo
    // Serial.print("$J=G21G91");
    // Serial.print(axis[key-1] );
    // Serial.print(step_size);
    // Serial.print("F");
    // Serial.println(feed);
    gcodeString = String("$J=G21G91" + axis[key-1]+step_size+"F"+feed);
    Serial.println(gcodeString);
}
void jogRoutine(KeypadEvent key) {
    kpadState = keypad.getState( );
    switch( kpadState ) {
        case PRESSED:
                //float step_size=0.1;
                Serial.println("on jog PRESSED");
                if (key <9){ //minore di nove è movimento
                      continue_move=false;
                      move_axis(key,0.1);
                }

                 break;
        case HOLD:
                Serial.println("on jog HOLD");
                if (key <9){ //minore di nove è movimento
                      continue_move=true;
                      move_axis(key,1.0);
                }
            break;

        case RELEASED:
             Serial.println("on jog RELEASED");
            Serial.println("stop jog");
            continue_move=false;
               
    }  // end switch-case
}// end switch on state function



void keypadEvent_routine(KeypadEvent key) {
    // stabilire il modo
    switch( status ) {
        case JOG:
              Serial.println("JOG");
              jogRoutine(key);
              break;
        case RUN:
              Serial.println("run");
              
              break;
        case ZERO:
              Serial.println("zero");
             
              
     }
}
void squareCenter(){
//procedura guidata per trovare il centro 
//di un pezzo abbastanza squadrato usando 4 punti sugli assi XeY
//i punti vengono presi girando intorno al pezzo in senso orario
//passo 1 :toccare il lato destro
//2:premere e tenere premuto il tasto centrale 5 convalida
/////azzerare X oppure prenderne nota e dare convalida alla fine

//toccare il lato posteriore
//hold 5
/////azzerare Y oppure prenderne nota e dare convalida alla fine
//toccare il lato sinistro
//hold 5
///////dimezzare X o convalida alla fine
//toccare il lato anteriore
//hold 5
///////dimezzare Y o convalida alla fine
//convalidare oppure 
//le coordinate sono aggiornale allo 0

}
void circleCenter(){
//procedura guidata per trovare il centro 
//di un pezzo circolare usando 3 o 4 punti sugli assi XeY

}
String return_string;
bool return_complete=false;
void loop() {
  //controlla il ritorno da grbl per aggiornare coordinate

    if (return_complete   ){
               return_complete=false;
               Serial.println("return_string");
                     }
              //  decode_string();
              //display_coordinate_status(10.0,20.0,30.0,40.0);
        
        
  

//linvio dei comandi deve avvenire in questa sezione per scandire 
//i giusti intervalli di tempo e non saturare il buffer di comandi che 
//possono far avanzare troppo la macchina

//creare delle maschere con i comandi pronti 
//o determinare le funzioni in base al tasto???
//qui ho il tasto e lo stato del tast e il modo di funzionamento

 if (continue_move){
    Serial.println(gcodeString);
    delay(500);//questo valore deve variare in accordo con il feed 
              //per ottenere un equilibrio tra lo stato del buffer
              //e il moto dell'asse in modo da fermarsi subito dopo il release
  }
  byte key = keypad.getKey();
 if (key){
   Serial.println(char(key));
 


  }


}
/*------------------------------------------------------------------------------
	The status line can take these various states:
	$10=1
	<Idle|MPos:0.000,0.000,0.000|FS:0,0|WCO:0.000,0.000,0.000>
	<Idle|MPos:0.000,0.000,0.000|FS:0,0|Ov:100,100,100>
	<Idle|MPos:0.000,0.000,0.000|FS:0,0>
	$10=2
	<Idle|WPos:0.000,0.000,0.000|Bf:15,128|FS:0,0|WCO:0.000,0.000,0.000>
	<Idle|WPos:0.000,0.000,0.000|Bf:15,128|FS:0,0|Ov:100,100,100>
	<Idle|WPos:0.000,0.000,0.000|Bf:15,128|FS:0,0>
--------------------------------------------------------------------------------
*/

void decode_string(){
    Serial.println(return_string);
    int startStatPos =return_string.indexOf("WCO");
    if (startStatPos==-1) return;

		int endStatPos = return_string.indexOf(">");
		return_string = return_string.substring(startStatPos+1, endStatPos-0);
    Serial.println(return_string);
}
	
void display_coordinate_status(float X,float Y,float Z,float A) {
   display.clearDisplay(); // Clear display buffer    
   display.setTextSize(2);             // Normal 1:1 pixel scale
   display.setTextColor(SSD1306_WHITE);        // Draw white text
   display.setCursor(0,0);             // Start at top-left corner
   display.print("X ");
   display.println(X);
   display.print("Y ");
   display.println(Y);
   display.print("Z ");
   display.println(Z);
   display.print("A ");
   display.println(A);
   display.display(); // Update screen with each newly-drawn line
}


void serialEvent() {
  while (Serial.available()) {
    // get the new byte:
    char inChar = (char)Serial.read();
    // add it to the inputString:
    return_string += inChar;
    // if the incoming character is a newline, set a flag so the main loop can
    // do something about it:
    if (inChar == '\n') {
      //controlla che ci sia wpos
      
         int startPosString = return_string.indexOf("WCO");
         Serial.println(startPosString);
          Serial.println(return_string);
         if (startPosString==-1){
                     return_string="";
              }else{
           Serial.println(startPosString); 

      return_complete = true;
    }
  }
}
}