From 958a337032a13f7b918dcc39b27b483a27121521 Mon Sep 17 00:00:00 2001
From: "Collin J. Doering" <collin.doering@rekahsoft.ca>
Date: Sat, 24 Oct 2015 00:42:54 -0400
Subject: [PATCH] Add cursor save and restore ANSI sequence

Add support for ANSI escape sequence for saving the current location of
the cursor "\e[s" and for recalling the last saved location "\e[u".

Note: if no location had been saved yet and the restore cursor
position (RPC) ANSI escape is received, defaults to 1,1 (top left of the
screen).

Signed-off-by: Collin J. Doering <collin.doering@rekahsoft.ca>
---
 lcdLib.c | 27 ++++++++++++++++++++++++++-
 lcdLib.h | 10 ++++++++++
 2 files changed, 36 insertions(+), 1 deletion(-)

diff --git a/lcdLib.c b/lcdLib.c
index d28ef5d..08e1377 100644
--- a/lcdLib.c
+++ b/lcdLib.c
@@ -30,6 +30,9 @@
 volatile uint8_t currentLineNum;
 volatile uint8_t currentLineChars;
 
+volatile uint8_t saveCursorLineNum;
+volatile uint8_t saveCursorLineChars;
+
 const uint8_t lineBeginnings[LCD_NUMBER_OF_LINES] = { LCD_LINE_BEGINNINGS };
 
 //------------------------------------------------------------------------------------------
@@ -277,10 +280,22 @@ void writeStringToLCD(char* str) {
     // Check for ANSI CSI (Control Sequence Introducer)
     if (*str == '\e') {
       if (*(++str) != '\0' && *str == '[') {
+        char* str_ref = ++str;
+        switch (*str) {
+        case 's': // SCP - Save cursor position
+          saveCursorPosition();
+          return;
+        case 'u': // RCP - Restore cursor position
+          restoreCursorPosition();
+          return;
+        default:
+          break;
+        }
+
         // Read optional variable length number in ASCII (0x30 - 0x3f) where 0x3a - 0x3f are
         // ignored (they are used as flags by some terminals)
         uint8_t fnd0;
-        uint8_t num0 = readASCIINumber(++str, &fnd0, &str);
+        uint8_t num0 = readASCIINumber(str, &fnd0, &str);
 
         // Read optional (semicolon followed by optional variable length number)
         uint8_t fnd1;
@@ -454,6 +469,16 @@ void moveCursorToColumn(uint8_t n) {
   } // else index out of range (off screen column)
 }
 
+void saveCursorPosition() {
+  saveCursorLineNum = currentLineNum;
+  saveCursorLineChars = currentLineChars;
+}
+
+void restoreCursorPosition() {
+  currentLineNum = saveCursorLineNum;
+  currentLineChars = saveCursorLineChars;
+  writeLCDInstr(INSTR_DDRAM_ADDR | (lineBeginnings[currentLineNum] + currentLineChars));
+}
 //-----------------------------------------------------------------------------------------------
 
 /* char readCharFromLCD(void) { */
diff --git a/lcdLib.h b/lcdLib.h
index dea3256..8c55a1e 100644
--- a/lcdLib.h
+++ b/lcdLib.h
@@ -139,6 +139,16 @@ void writeCharToLCD(char);
  */
 void writeStringToLCD(char*);
 
+/**
+   Saves the cursors current position.
+ */
+void saveCursorPosition(void);
+
+/**
+   Restores the last saved cursor position.
+ */
+void restoreCursorPosition(void);
+
 //-----------------------------------------------------------------------------------------------
 // LCD command functions