#include <LiquidCrystal_I2C.h>
#include <Keypad.h>
#include <SD.h>
#include "protothreads.h"
#include <IRremote.h>

#define SPI_SPEED SD_SCK_MHZ(4)
#define CS_PIN 53
#define PIN_RECEIVER 41   // Signal Pin of IR receiver

IRrecv receiver(PIN_RECEIVER);
byte pass = 0;

//******variables*************
int skip = 0;
char grbl_snapshot[120] = " ";
char file[12][11];// 8.3 "limit" imposed by FAT filesystem
char file_tmp[12][11];
byte findex = 0; //file index
byte file_offset = 0;
char file_dis[12][20];
byte dis_row;
char display_line[4][20];
byte num_files;
long size[4];
byte query_sent = 0;
byte position_query_sent = 0;
byte query_response = 0;
char grbl_Status[60] = "";
char status[200] = "";
String code_parsed;

byte alt_mode = 0;
byte CNC_alarm = 0;
byte CNC_locked = 0;
byte CNC_enable = 0;
byte CNC_buffer_ready = 0;
byte CNC_stream_gcode = 0;
byte run_query_sent = 0;
//byte CNC_idle;
byte CNC_cycle;
byte kp_num;
signed int mindex;    //grbl message index

byte index = 0;
byte dindex;
int sp_spd[] = {400, 800, 1200, 1600, 2000, 2400, 2800, 3200};
signed int spindex;         //spindle speed index
byte fd_rate[] = {30, 50, 70, 90, 110, 130, 150, 170, 190, 210};
signed int fdindex = 0;     //feed rate index
byte feed_hold = 0;

byte file_mode;
byte jog_mode;

float jdelta = 0.1;
float x_offset = 137;
float y_offset = -55;
float z_offset = 0;

//******end variables*************

//******prototypes*************
void start_spindle();
void stop_spindle();

void grbl_pause();
void CNC_reset();
void CNC_unlock();
void Jog_Xup();
void Jog_Yup();
void Jog_Zup();
void Jog_Xdn();
void Jog_Ydn();
void Jog_Zdn();
void CNC_home();
void CNC_offset();
void Z_zero();
void XY_zero();
void get_offsets();
void Cyc_hold();
void Cyc_Start_Resume();
void display_jd();
void display_lp();
void SD_lp();
void files_lp();
void messenger_lp();
void kp_lp();
String query_motion_status();

//******end prototypes*************

//******LiquidCrystal_I2C*******************************************************
#define I2C_ADDR    0x27
#define LCD_COLUMNS 20
#define LCD_LINES   4
LiquidCrystal_I2C lcd(0x27, 20, 4);

//*****exec Keypad**************************************************************
const uint8_t ROWS = 4;
const uint8_t COLS = 4;
char exec_keys[ROWS][COLS] = {
  { '1', '2', '3', 'A' },
  { '4', '5', '6', 'B' },
  { '7', '8', '9', 'C' },
  { '*', '0', '#', 'D' }
};
uint8_t ecolPins[COLS] = { 31, 33, 35, 37 }; // Pins connected to C1, C2, C3, C4
uint8_t erowPins[ROWS] = { 23, 25, 27, 29 }; // Pins connected to R1, R2, R3, R4
Keypad exec_keypad = Keypad(makeKeymap(exec_keys), erowPins, ecolPins, ROWS, COLS);
//****************************************************************************

//*****manual Keypad**************************************************************
//const uint8_t ROWS = 4;
//const uint8_t COLS = 4;
char manual_keys[ROWS][COLS] = {
  { '1', '2', '3', 'A' },
  { '4', '5', '6', 'B' },
  { '7', '8', '9', 'C' },
  { '*', '0', '#', 'D' }
};
uint8_t mcolPins[COLS] = { 30, 32, 34, 36 }; // Pins connected to C1, C2, C3, C4
uint8_t mrowPins[ROWS] = { 22, 24, 26, 28 }; // Pins connected to R1, R2, R3, R4
Keypad manual_keypad = Keypad(makeKeymap(manual_keys), mrowPins, mcolPins, ROWS, COLS);
//****************************************************************************
File root;

String query_motion_status()
{
  Serial1.write('?');//send ?
  Serial1.write(0x0D);//send CR
  Serial1.write(0x0A);//send line feed \n
  //while (Serial1.available() == 0) {}        //wait for data available
  //String code_parsed = Serial1.readString(); //read until timeout
  //return code_parsed;
  return "<position data>";
}

void display_jd()
{
  lcd.clear(); // Clear the entire screen
  lcd.setCursor(0, 0);
  lcd.print("Jog Delta");
  lcd.setCursor(10, 0);
  lcd.print(jdelta);
  lcd.setCursor(0, 3);
  lcd.print("Press OK to clear");
}

//loop simulation
void display_lp() {}
void SD_lp() {}
void files_lp() {}
void messenger_lp() {}
void kp_lp() {}

void Jog_Xup() {
  CNC_cycle = 1;
}
void Jog_Yup() {
  CNC_cycle = 1;
}
void Jog_Zup()
{
  delay(50);
}
void Jog_Xdn() {
  CNC_cycle = 1;
}
void Jog_Ydn() {
  CNC_cycle = 1;
}
void Jog_Zdn() {
  CNC_cycle = 1;
}
void Z_zero() {}
void XY_zero() {}
void CNC_home() {}
void CNC_offset() {}
void Cyc_hold() {}
void Cyc_Start_Resume() {}
void start_spindle() {}
void stop_spindle() {}

void CNC_unlock() {}

void CNC_reset() {}

void get_offsets()
{}


//***********start keypad logic************************************
void exec_scan(char exec_key)
{ 
  switch (exec_key)
  { 
    case '1':
      kp_num = 1;                       //press [Alt] then [Offset Pos] key to initiate
      alt_mode++;                       //loading offset values from SD file.
      if (alt_mode > 1) //alt_mode > 1
      {
        alt_mode = 0;
      }
      lcd.clear(); // Clear the entire screen
      lcd.setCursor(0, 0);
      lcd.print("ALT Toggle Pushed ");
      lcd.setCursor(0, 1);
      lcd.print(alt_mode);
      break;
    case '2':
      kp_num = 2;
      if (CNC_cycle == 0 && CNC_alarm == 0)
      {
        file_mode = 1;
        //Serial.println("File Mode");
        //List SD root dir files
        root = SD.open("/");
        printDirectory(root,0);        
        move_curser();
      }
      break;
    case '3':
      kp_num = 3;
      if (file_mode == 1)
      {
        //Serial.println("File Selected");
        //Select SD file
        select_file();
      }
      break;
    case 'A':      
      kp_num = 4;
      //Serial.println("up");
      if (file_mode)
      {        
        file_offset--;

        if (file_offset == 255 )
        {
          file_offset = 0;
        }
      }
      move_curser();
      break;
    case '4':
      kp_num = 5;
      if (CNC_cycle == 0)
      {
        //Start spindle
        start_spindle();
      }
      break;
    case '5':
      kp_num = 6;                      //incr spindle speed index
      spindex++;
      if (spindex > 7 )
      {
        spindex = 7;
      }
      //show speed on display
      lcd.clear(); // Clear the entire screen
      lcd.setCursor(0, 0);
      lcd.print("Spindle Speed");
      lcd.setCursor(0, 1);
      lcd.print(sp_spd[spindex]);
      break;
    case '6':
      kp_num = 7;                      //Incr feed rate
      fdindex++;
      if (fdindex > 9 )
      {
        fdindex = 9;
      }
      //show speed on display
      lcd.clear(); // Clear the entire screen
      lcd.setCursor(0, 0);
      lcd.print("Feed Rate");
      lcd.setCursor(0, 1);
      lcd.print(fd_rate[fdindex]);
      break;
    case 'B':
      kp_num = 8;
      //Serial.println("dn");
      if (file_mode)        //Incr
      {
        file_offset++;
        if (file_offset > num_files-1 )
        {
          file_offset = num_files-1;
        }
      }
      move_curser();
      break;
    case '7':
      kp_num = 9;                       //stop spindle
      stop_spindle();
      break;
    case '8':
      kp_num = 10;                      //decr spindle speed index
      spindex--;
      if (spindex < 0 )
      {
        spindex = 0;
      }
      //show speed on display
      lcd.clear(); // Clear the entire screen
      lcd.setCursor(0, 0);
      lcd.print("Spindle Speed");
      lcd.setCursor(0, 1);
      lcd.print(sp_spd[spindex]);
      break;
    case '9':
      kp_num = 11;                      //Decr feed rate
      fdindex--;
      if (fdindex < 0 )
      {
        fdindex = 0;
      }
      lcd.clear(); // Clear the entire screen
      lcd.setCursor(0, 0);
      lcd.print("Feed Rate");
      lcd.setCursor(0, 1);
      lcd.print(fd_rate[fdindex]);
      break;
    case 'C':
      kp_num = 12;//Null
      break;
    case '*':
      if (CNC_cycle == 0)
      {
        //Send grbl code to CNC
        CNC_stream_gcode = 1;
        lcd.clear(); // Clear the entire screen
        lcd.setCursor(0, 0);
        lcd.print("Sending Gcode to CNC");
      }
      break;
    case '0':
      kp_num = 14;                      //Hold CNC process
      feed_hold = 1;
      Cyc_hold();
      lcd.clear(); // Clear the entire screen
      lcd.setCursor(0, 0);
      lcd.print("CNC Cycle on Hold");
      lcd.setCursor(0, 1);
      lcd.print("Press Cycle Resume");
      lcd.setCursor(0, 2);
      lcd.print("        or");
      lcd.setCursor(0, 3);
      lcd.print("Press OK to clear");
      break;
    case '#':
      kp_num = 15;
      if (feed_hold)
      {
        feed_hold = 0;
        Cyc_Start_Resume();           //Cycle Start/Resume Resumes a feed hold
        lcd.clear(); // Clear the entire screen
        lcd.setCursor(0, 0);
        lcd.print("CNC Cycle Resumed");
        lcd.setCursor(0, 1);
        lcd.print("Press OK to clear");
      }
      break;
    case 'D':
      kp_num = 16;//unlock CNC
      CNC_unlock();
      lcd.clear(); // Clear the entire screen
      lcd.setCursor(0, 0);
      lcd.print("CNC Unlocked");
      lcd.setCursor(0, 1);
      lcd.print("Press OK to clear");
      break;
  }
}//end

void manual_scan(char manual_key)
{
  switch (manual_key)
  {
    case '1':
      kp_num = 17; //Jog Z up
      if (CNC_alarm == 0 && CNC_cycle == 0)
      {
        lcd.clear(); // Clear the entire screen
        lcd.setCursor(0, 0);
        lcd.print("Jogging Z Up");
        lcd.setCursor(0, 1);
        lcd.print("Jog Delta");
        lcd.setCursor(10, 1);
        lcd.print(jdelta);
        lcd.setCursor(0, 2);
        lcd.print("Press OK to clear");
        CNC_cycle = 1;
        Jog_Zup();
        if (1)
        {
          Serial.println("<Idle>");
          query_sent = 1;
        }
      }
      break;
    case '2':
      kp_num = 18;//Jog Y up
      if (CNC_alarm == 0 && CNC_cycle == 0)
      {
        Jog_Yup();
        lcd.clear(); // Clear the entire screen
        lcd.setCursor(0, 0);
        lcd.print("Jogging Y Up");
        lcd.setCursor(0, 1);
        lcd.print("Jog Delta");
        lcd.setCursor(10, 1);
        lcd.print(jdelta);
        lcd.setCursor(0, 2);
        lcd.print("Press OK to clear");
      }
      break;
    case '3':
      kp_num = 19;
      jdelta = 0.1; // [0.1]
      display_jd();
      break;
    case 'A':
      kp_num = 20;
      jdelta = 1; // [1.0]
      display_jd();
      break;
    case '4':
      kp_num = 21;
      if (CNC_alarm == 0 && CNC_cycle == 0)
      {
        // Jog X Down
        Jog_Xdn();
        lcd.clear(); // Clear the entire screen
        lcd.setCursor(0, 0);
        lcd.print("Jogging X Dn");
        lcd.setCursor(0, 1);
        lcd.print("Jog Delta");
        lcd.setCursor(10, 1);
        lcd.print(jdelta);
        lcd.setCursor(0, 2);
        lcd.print("Press OK to clear");
      }
      break;
    case '5':
      kp_num = 22; //OK, clear file directory
      //lcd.clear(); // Clear the entire screen
      if (file_mode == 1)
      {
        file_mode = 0;
        lcd.clear(); // Clear the entire screen
        lcd.setCursor(0, 0);
        lcd.print("File Mode Ended");
      }
      if (CNC_cycle == 1)
      {
        CNC_cycle = 0;
        lcd.clear(); // Clear the entire screen
        lcd.setCursor(0, 0);
        lcd.print("Cycle Mode Ended");
      }
      delay(1000);
      lcd.clear();
      break;
      break;
    case '6':
      kp_num = 23;
      //Jog X Up
      if (CNC_alarm == 0 && CNC_cycle == 0)
      {
        Jog_Xup();
        lcd.clear(); // Clear the entire screen
        lcd.setCursor(0, 0);
        lcd.print("Jogging X Up");
        lcd.setCursor(0, 1);
        lcd.print("Jog Delta");
        lcd.setCursor(10, 1);
        lcd.print(jdelta);
        lcd.setCursor(0, 2);
        lcd.print("Press OK to clear");
      }
      break;
    case 'B':
      kp_num = 24;
      jdelta = 5; // [5.0]
      display_jd();
      break;
    case '7':
      kp_num = 25; //Jog Z Down
      if (CNC_alarm == 0 && CNC_cycle == 0)
      {
        Jog_Zdn();
        lcd.clear(); // Clear the entire screen
        lcd.setCursor(0, 0);
        lcd.print("Jogging Z Dn");
        lcd.setCursor(0, 1);
        lcd.print("Jog Delta");
        lcd.setCursor(10, 1);
        lcd.print(jdelta);
        lcd.setCursor(0, 2);
        lcd.print("Press OK to clear");
      }
      break;
    case '8':
      kp_num = 26;//Jog Y Down
      if (CNC_alarm == 0 && CNC_cycle == 0)
      {
        Jog_Ydn();
        lcd.clear(); // Clear the entire screen
        lcd.setCursor(0, 0);
        lcd.print("Jogging Y Dn");
        lcd.setCursor(0, 1);
        lcd.print("Jog Delta");
        lcd.setCursor(10, 1);
        lcd.print(jdelta);
        lcd.setCursor(0, 2);
        lcd.print("Press OK to clear");
      }
      break;
    case '9':
      kp_num = 27;
      jdelta = 10; // [10.0]
      display_jd();
      break;
    case 'C':
      kp_num = 28;
      if (CNC_alarm == 0 && CNC_cycle == 0)
      {
        XY_zero();  //set current XY pos as zero datum
        lcd.clear(); // Clear the entire screen
        lcd.setCursor(0, 0);
        lcd.print("Setting XY");
        lcd.setCursor(0, 1);
        lcd.print("Zero Datum");
        lcd.setCursor(0, 3);
        lcd.print("Press OK to clear");
      }
      break;
    case '*':
      kp_num = 29;
      if (CNC_alarm == 0 && CNC_cycle == 0)
      {
        CNC_home();  //code to home CNC
        lcd.clear(); // Clear the entire screen
        lcd.setCursor(0, 0);
        lcd.print("Moving to Home Pos");
        lcd.setCursor(0, 3);
        lcd.print("Press OK to clear");
      }
      break;
    case '0':
      kp_num = 30;
      if (CNC_cycle == 0 && alt_mode == 1)
      {
        lcd.clear(); // Clear the entire screen
        lcd.setCursor(0, 0);
        lcd.print("Getting Offsets");
        get_offsets();                //load offsets from SD.
      }
      else
      {
        CNC_offset();                 //code to move CNC to offset position
        lcd.clear(); // Clear the entire screen
        lcd.setCursor(0, 0);
        lcd.print("Moving to Offset Pos");
        lcd.setCursor(0, 3);
        lcd.print("Press OK to clear");
      }
      break;
    case '#':
      kp_num = 31;
      CNC_reset();                      //send Reset to CNC
      lcd.clear(); // Clear the entire screen
      lcd.setCursor(0, 0);
      lcd.print("Reset Sent to");
      lcd.setCursor(0, 1);
      lcd.print("grbl CNC");
      lcd.setCursor(0, 3);
      lcd.print("Press OK to clear");
      break;
    case 'D':
      kp_num = 32;
      if (CNC_alarm == 0 && CNC_cycle == 0)
      {
        Z_zero(); //set current Z pos as zero datum
        lcd.clear(); // Clear the entire screen
        lcd.setCursor(0, 0);
        lcd.print("Setting Z");
        lcd.setCursor(0, 1);
        lcd.print("Zero Datum");
        lcd.setCursor(0, 3);
        lcd.print("Press OK to clear");
      }
      break;
  }
}
//**************************end keypad logic************************************

void select_file()
{
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print(file[file_offset]);
  lcd.setCursor(0, 1);
  lcd.print("Selected");
}//end select_file

/*                    
  ********************************
          LCD Display map
  ********************************          
      |0|1|2|3|4|5|6|7|8|9|10|11|12|13|14|15|16|17|18|19|        
 row0 |>|$|$|$|$|$|$|$| | |  |$ |$ |$ |$ |$ |$ |$ |  |  |  
 row1 | |$|$|$|$|$|$|$| | |  |$ |$ |$ |$ |$ |$ |$ |  |  |
 row2 | |$|$|$|$|$|$|$| | |  |$ |$ |$ |$ |$ |$ |$ |  |  |
 row3 | |$|$|$|$|$|$|$| | |  |$ |$ |$ |$ |$ |$ |$ |  |  |
*/
//curser is positioned up and down display columns 2 and 12
signed int move_curser() 
{ 
  //Serial.print("file offset ");
  //Serial.println(file_offset);
  //clear cursers              
  for(int i=0;i < 4;i++)
    {
      lcd.setCursor(0, i);
      lcd.print(" ");
      lcd.setCursor(10, i);
      lcd.print(" ");
    }  
  switch (file_offset)
  { 
    case 0:      
      lcd.setCursor(0, 0);
      lcd.print(">");
      break;
    case 1:      
      lcd.setCursor(0, 1);
      lcd.print(">");
      break;    
    case 2:      
      lcd.setCursor(0, 2);
      lcd.print(">");
      break;
    case 3:      
      lcd.setCursor(0, 3);
      lcd.print(">");
      break;  
    case 4:      
      lcd.setCursor(10, 0);
      lcd.print(">");
      break;
    case 5:      
      lcd.setCursor(10, 1);
      lcd.print(">");
      break;
    case 6:      
      lcd.setCursor(10, 2);
      lcd.print(">");
      break;
    case 7:      
      lcd.setCursor(10, 3);
      lcd.print(">");
      break;
  }  
}


//write file names display
void printDirectory(File dir, int numTabs)
{ 
  dis_row=0; 
  lcd.clear();
  while (true)
  {
    File entry =  dir.openNextFile();
    
    if (!entry)
    {
      num_files = findex;      
      // no more files
      findex = 0;
      break;
    }
    //save file names, room for 12 entries
    if (strcmp(entry.name(), "SYSTEM~1") != 0)      // CAP_EMB.NC
      {  
        strcpy(file_dis[dis_row],"");        
        strcpy(file[findex], entry.name());
        strcpy(file_tmp[findex], entry.name());

        //get filename, clip extension
        //file[12][11] 8.3 "limit" imposed by FAT filesystem
        if(findex < 7)
          { 
              int i;              
              for(i=0;i < strlen(file_tmp[findex]);i++)
                {
                    if(file_tmp[findex][i] == '.') 
                      {
                        file_tmp[findex][i] ='\0';
                        //strcat(file_dis[dis_row],file_tmp[findex][i]);
                        break;
                      }                                    
                }
              strcat(file_dis[dis_row],file_tmp[findex]);                        
          }           
        
        //Serial.print(file_dis[dis_row]);
        if(findex < 4)
          {
          lcd.setCursor(2, findex);
          lcd.print(file_dis[dis_row]); 
          } 
        if(findex > 3)
          {
          lcd.setCursor(12, findex-4);
          lcd.print(file_dis[dis_row]); 
          }  

        findex++;

        if(dis_row < 4)
          {dis_row++;}
          
        if(dis_row == 4)
          {dis_row=0;}

      }//end if (strcmp(entry.name(),
    entry.close();
  }//end while
}//end printDirectory

//**************start multitask logic**************************
pt ptkeypad;
int keypadThread(struct pt* pt)
{
  PT_BEGIN(pt);
  // Loop forever
  for (;;)
  {
    char exec_key = exec_keypad.getKey();
    if (exec_key != NO_KEY)
    { //Serial.println("Key Selected");
      exec_scan(exec_key);
    }

    char manual_key = manual_keypad.getKey();
    if (manual_key != NO_KEY)
    {
      manual_scan(manual_key);
    }
    PT_YIELD(pt);
  }
  PT_END(pt);
}

pt ptcommutate;
int commutateThread(struct pt* pt)
{
  PT_BEGIN(pt);
  for (;;) // Loop forever
  { Serial.print("commutating");
    display_lp();
    SD_lp();
    files_lp();
    messenger_lp();
    kp_lp();
    PT_YIELD(pt);
  }//end for loop
  PT_END(pt);
}

pt ptblink;
int blinkThread(struct pt* pt)
{
  PT_BEGIN(pt);
  for (;;) // Loop forever
  {
    if (CNC_cycle == 1) //
    {
      digitalWrite(4, HIGH);
      PT_SLEEP(pt, 1000);
      digitalWrite(4, LOW);
      PT_SLEEP(pt, 1000);
    }
    if (CNC_cycle == 0) //
    {
      //Serial.println("idle state");
      digitalWrite(4, HIGH);
    }
    PT_YIELD(pt);
  }//end for loop
  PT_END(pt);
}

pt ptenable;
int enableThread(struct pt* pt)
{
  PT_BEGIN(pt);
  for (;;) // Loop forever
  {
    if (digitalRead(3) == 0)
    {
      CNC_alarm = 1;
      CNC_cycle = 0;
    }
    else
    {
      CNC_alarm = 0;
    }
    if (digitalRead(2) == 0)
    {
      CNC_enable = 1;
    }
    else
    {
      CNC_enable = 0;
    }
    if (CNC_alarm == 1)
    {
      digitalWrite(5, HIGH);
    }
    else
    {
      digitalWrite(5, LOW);
    }

    //simulate OK message by actuating PB sw
    if (digitalRead(6) == HIGH)
    {
      query_response = 2;
      //digitalWrite(13,HIGH);
    }
    else
    {
      //digitalWrite(13,LOW);
    }
    PT_YIELD(pt);
  }//end for loop
  PT_END(pt);
}

pt ptquery;
int queryThread(struct pt* pt)
{
  PT_BEGIN(pt);
  for (;;) // Loop forever
  {
    if (position_query_sent == 1)
    { 
      if (Serial1.available() != 0)
      {
        digitalWrite(13, HIGH);
      }
      else
      {
        digitalWrite(13, LOW);
      }

      if (Serial1.available() != 0)
      { 
        /* 
        //Serial.println(Serial1.available());
        for (int i = 0; i < 120; i++)
        {
          status[i] = Serial1.read();
          Serial.print(status[i]);
          if (status[i] == '\r')
            {
              status[i] = '\0'; // Append a null
              break;
            }
        }        
        */ 
        code_parsed = Serial1.readString();
        code_parsed.replace("<", "");
        code_parsed.replace(">", "");
        code_parsed.replace("MPos", "M");
        code_parsed.replace("WCO", "W");
        code_parsed.toCharArray(status,200);
        position_query_sent=0;
        query_response = 6;
      }
    }

    PT_YIELD(pt);
  }//end for loop
  PT_END(pt);
}
pt ptrealtime;
int realtimeThread(struct pt* pt)
{
  PT_BEGIN(pt);
  for (;;) // Loop forever
  {
    PT_SLEEP(pt,200);
    //scan for status
    Serial1.write('?');
    position_query_sent = 1;

    PT_YIELD(pt);
  }//end for loop
  PT_END(pt);
}

//**************end multitask logic**************************

void setup()
{
  PT_INIT(&ptcommutate);
  PT_INIT(&ptkeypad);
  PT_INIT(&ptblink);
  PT_INIT(&ptquery);
  PT_INIT(&ptrealtime);
  PT_INIT(&ptenable);

  receiver.enableIRIn(); // Start the receiver

  pinMode(2, INPUT_PULLUP);  //enable operation
  pinMode(3, INPUT_PULLUP);  //inhibit operation
  pinMode(4, OUTPUT); // cycle: blinking/idle: continuous
  pinMode(5, OUTPUT); // alarm
  pinMode(41, INPUT_PULLUP);
  // Open serial communications and wait for port to open:
  Serial.begin(115200);
  while (!Serial)
  {
    ;  // wait for serial port to connect. Needed for Native USB only
  }
  Serial1.begin(115200, SERIAL_8N1);

  lcd.init();       // Initialize the LCD
  lcd.backlight();  // Turn on the LCD backlight

  if (!SD.begin(CS_PIN))
  {
    Serial.println("Card initialization failed!");
  }
  pinMode(6, INPUT);
  pinMode(7, OUTPUT);
  pinMode(13, OUTPUT);
  digitalWrite(7, HIGH);
  //Serial.println("****************");
  code_parsed.reserve(63);
} //end setup

void loop()
{
  PT_SCHEDULE(keypadThread(&ptcommutate));
  PT_SCHEDULE(keypadThread(&ptkeypad));
  PT_SCHEDULE(blinkThread(&ptblink));
  PT_SCHEDULE(enableThread(&ptenable));
  PT_SCHEDULE(queryThread(&ptquery));
  PT_SCHEDULE(realtimeThread(&ptrealtime));

  // Checks received an IR signal
  if (receiver.decode())
  {
    translateIR();
    receiver.resume();  // Receive the next value
  }

 /* 
  if (query_response == 6)
    { 
    byte i = 1;
    char state[10];
    byte ptr[6];
    byte status_size;
    char *pointer;
    const char *delimiter = "|";

    //determine size of status input
    for (byte j = 0; j < 200; j++)
    {
      state[j] = status[j];
      if (state[j] == '\r')
      {
        status_size=j;
        break;
      }
    }
    
    
    pointer = strtok(status, delimiter);
    while (pointer != NULL)
    {
      if (i > 3) 
      {
        break;
      }
      ptr[i] = pointer;
      pointer = strtok(NULL, delimiter);
      strcpy(display_line[i],pointer);
      i++;
    }

    for (byte j = 0; j < 20; j++)
      {
        state[j] = status[j];
        if (state[j] == '|')
        {
          state[j] = '\0';
          break;
        }
      }
    
    strcpy(display_line[0],state);
    
    line_display();    
    query_response = 0;
  }

  if (query_response == 2 && CNC_stream_gcode == 1)
    {
    Serial.println(CNC_stream_gcode);
    lcd.clear(); // Clear the entire screen
    lcd.setCursor(0, 0);
    lcd.print("gcode -> grbl");
    lcd.setCursor(0, 1);
    lcd.print("G0 X0Y0Z0");
    query_response = 0;
    CNC_buffer_ready = 1;
    }
*/
//display x on line 1, y on line 2, z on line 3
  
  if(1)
    { 
      byte i = 0;
      char *token;
      char *mystring = "100.1,200.2,10.3";
      const char *delimiter =",";
      lcd.setCursor(0,0);
      lcd.print("X ");
      lcd.setCursor(0,1);
      lcd.print("Y ");
      lcd.setCursor(0,2);
      lcd.print("Z ");
      token = strtok(mystring, delimiter);

      while (token != NULL) 
        { 
          if (i > 3) 
            {
              break;
            }          

          //Serial.println(token);
          lcd.setCursor(2,i);
          lcd.print(token);
          token=strtok(NULL, delimiter);
          i++;
        }
      query_response=0;        
    }

} //end loop

void line_display()
{
  lcd.clear();
  for(int k=0;k<4;k++)
    {
      lcd.setCursor(0, k);
      lcd.print(display_line[k]);    
    }   
}

void translateIR()
{
  // Takes command based on IR code received
  switch (receiver.decodedIRData.command) 
    {
    case 176:
      lcd.clear();
      break;
    case 48:
      //lcd.print("num: 1");
      //delay(500);
      //lcd.clear();
      Serial.println("<Idle|MPos:0.000,0.000,0.000|FS:0,0|WCO:0.000,0.000,0.000>");
      break;
    case 24:
      //lcd.print("num: 2");
      //delay(500);
      //lcd.clear();
      Serial.println("<Run|W:17.147,-3.604,-3.857|Bf:0,122|FS:200,0>");
      break;
    case 122:
      //lcd.print("num: 3");
      //delay(500);
      //lcd.clear();
      Serial.println("<Alarm|MPos:0.000,0.000,0.000|FS:0,0|WCO:0.000,0.000,0.000>");
      break;
    case 16:
      lcd.print("num: 4");
      break;
    case 56:
      lcd.print("num: 5");
      break;
    case 90:
      lcd.print("num: 6");
      break;   
    default:
      lcd.clear();
      lcd.print(receiver.decodedIRData.command);
      lcd.print(" other button");
    }
}