Add vstr_ins and vstr_cut_out; improve stmhal readline.

8 files changed, 175 insertions, 38 deletions

diff --git a/py/builtinimport.c b/py/builtinimport.c
index e64ca82374..7cbb46012b 100644
--- a/py/builtinimport.c
+++ b/py/builtinimport.c
@@ -211,13 +211,13 @@ mp_obj_t mp_builtin___import__(int n_args, mp_obj_t *args) {
                     vstr_add_char(&path, PATH_SEP_CHAR);
                     vstr_add_str(&path, "__init__.py");
                     if (mp_import_stat(vstr_str(&path)) != MP_IMPORT_STAT_FILE) {
-                        vstr_cut_tail(&path, sizeof("/__init__.py") - 1); // cut off /__init__.py
+                        vstr_cut_tail_bytes(&path, sizeof("/__init__.py") - 1); // cut off /__init__.py
                         nlr_jump(mp_obj_new_exception_msg_varg(&mp_type_ImportError,
                             "Per PEP-420 a dir without __init__.py (%s) is a namespace package; "
                             "namespace packages are not supported", vstr_str(&path)));
                     }
                     do_load(module_obj, &path);
-                    vstr_cut_tail(&path, sizeof("/__init__.py") - 1); // cut off /__init__.py
+                    vstr_cut_tail_bytes(&path, sizeof("/__init__.py") - 1); // cut off /__init__.py
                 } else { // MP_IMPORT_STAT_FILE
                     do_load(module_obj, &path);
                     // TODO: We cannot just break here, at the very least, we must execute
diff --git a/py/lexer.c b/py/lexer.c
index be0b1883c5..26fec121c1 100644
--- a/py/lexer.c
+++ b/py/lexer.c
@@ -516,7 +516,7 @@ STATIC void mp_lexer_next_token_into(mp_lexer_t *lex, mp_token_t *tok, bool firs
         }
 
         // cut off the end quotes from the token text
-        vstr_cut_tail(&lex->vstr, n_closing);
+        vstr_cut_tail_bytes(&lex->vstr, n_closing);
 
     } else if (is_head_of_identifier(lex)) {
         tok->kind = MP_TOKEN_NAME;
diff --git a/py/misc.h b/py/misc.h
index 3671f42d87..19a21d52e7 100644
--- a/py/misc.h
+++ b/py/misc.h
@@ -63,8 +63,8 @@ bool unichar_isxdigit(unichar c);
 /** variable string *********************************************/
 
 typedef struct _vstr_t {
-    int alloc;
-    int len;
+    uint alloc;
+    uint len;
     char *buf;
     bool had_error : 1;
     bool fixed_buf : 1;
@@ -94,7 +94,11 @@ void vstr_add_str(vstr_t *vstr, const char *str);
 void vstr_add_strn(vstr_t *vstr, const char *str, int len);
 //void vstr_add_le16(vstr_t *vstr, unsigned short v);
 //void vstr_add_le32(vstr_t *vstr, unsigned int v);
-void vstr_cut_tail(vstr_t *vstr, int len);
+void vstr_ins_byte(vstr_t *vstr, uint byte_pos, byte b);
+void vstr_ins_char(vstr_t *vstr, uint char_pos, unichar chr);
+void vstr_cut_head_bytes(vstr_t *vstr, uint bytes_to_cut);
+void vstr_cut_tail_bytes(vstr_t *vstr, uint bytes_to_cut);
+void vstr_cut_out_bytes(vstr_t *vstr, uint byte_pos, uint bytes_to_cut);
 void vstr_printf(vstr_t *vstr, const char *fmt, ...);
 
 /** non-dynamic size-bounded variable buffer/string *************/
diff --git a/py/vstr.c b/py/vstr.c
index 6630ac40a7..90c3c06e2c 100644
--- a/py/vstr.c
+++ b/py/vstr.c
@@ -159,12 +159,12 @@ char *vstr_add_len(vstr_t *vstr, int len) {
     return buf;
 }
 
-void vstr_add_byte(vstr_t *vstr, byte v) {
+void vstr_add_byte(vstr_t *vstr, byte b) {
     byte *buf = (byte*)vstr_add_len(vstr, 1);
     if (buf == NULL) {
         return;
     }
-    buf[0] = v;
+    buf[0] = b;
 }
 
 void vstr_add_char(vstr_t *vstr, unichar c) {
@@ -214,7 +214,48 @@ void vstr_add_le32(vstr_t *vstr, unsigned int v) {
 }
 */
 
-void vstr_cut_tail(vstr_t *vstr, int len) {
+char *vstr_ins_blank_bytes(vstr_t *vstr, uint byte_pos, uint byte_len) {
+    if (vstr->had_error) {
+        return NULL;
+    }
+    uint l = vstr->len;
+    if (byte_pos > l) {
+        byte_pos = l;
+    }
+    if (byte_len > 0) {
+        // ensure room for the new bytes
+        if (!vstr_ensure_extra(vstr, byte_len)) {
+            return NULL;
+        }
+        // copy up the string to make room for the new bytes
+        memmove(vstr->buf + l - 1 + byte_len, vstr->buf + l - 1, l - byte_pos);
+        // increase the length
+        vstr->len += byte_len;
+        vstr->buf[vstr->len] = 0;
+    }
+    return vstr->buf + byte_pos;
+}
+
+void vstr_ins_byte(vstr_t *vstr, uint byte_pos, byte b) {
+    char *s = vstr_ins_blank_bytes(vstr, byte_pos, 1);
+    if (s != NULL) {
+        *s = b;
+    }
+}
+
+void vstr_ins_char(vstr_t *vstr, uint pos, unichar chr) {
+    // TODO UNICODE
+    char *s = vstr_ins_blank_bytes(vstr, pos, 1);
+    if (s != NULL) {
+        *s = chr;
+    }
+}
+
+void vstr_cut_head_bytes(vstr_t *vstr, uint bytes_to_cut) {
+    vstr_cut_out_bytes(vstr, 0, bytes_to_cut);
+}
+
+void vstr_cut_tail_bytes(vstr_t *vstr, uint len) {
     if (vstr->had_error) {
         return;
     }
@@ -226,6 +267,19 @@ void vstr_cut_tail(vstr_t *vstr, int len) {
     vstr->buf[vstr->len] = 0;
 }
 
+void vstr_cut_out_bytes(vstr_t *vstr, uint byte_pos, uint bytes_to_cut) {
+    if (vstr->had_error || byte_pos >= vstr->len) {
+        return;
+    } else if (byte_pos + bytes_to_cut >= vstr->len) {
+        vstr->len = byte_pos;
+        vstr->buf[vstr->len] = 0;
+    } else {
+        // move includes +1 for null byte at the end
+        memmove(vstr->buf + byte_pos, vstr->buf + byte_pos + bytes_to_cut, vstr->len - byte_pos - bytes_to_cut + 1);
+        vstr->len -= bytes_to_cut;
+    }
+}
+
 void vstr_printf(vstr_t *vstr, const char *fmt, ...) {
     va_list ap;
     va_start(ap, fmt);
diff --git a/stm/pyexec.c b/stm/pyexec.c
index 75fe87c88b..e3e50441dd 100644
--- a/stm/pyexec.c
+++ b/stm/pyexec.c
@@ -88,7 +88,7 @@ int readline(vstr_t *line, const char *prompt) {
                 escape = true;
             } else if (c == 127) {
                 if (vstr_len(line) > len) {
-                    vstr_cut_tail(line, 1);
+                    vstr_cut_tail_bytes(line, 1);
                     stdout_tx_str("\b \b");
                 }
             } else if (32 <= c && c <= 126) {
diff --git a/stmhal/pyexec.c b/stmhal/pyexec.c
index ecdd4d8c6d..f1aa7dcad4 100644
--- a/stmhal/pyexec.c
+++ b/stmhal/pyexec.c
@@ -38,6 +38,16 @@ void stdout_tx_str(const char *str) {
     usb_vcp_send_str(str);
 }
 
+void stdout_tx_strn(const char *str, uint len) {
+    if (pyb_usart_global_debug != PYB_USART_NONE) {
+        usart_tx_strn(pyb_usart_global_debug, str, len);
+    }
+#if defined(USE_HOST_MODE) && MICROPY_HW_HAS_LCD
+    lcd_print_strn(str, len);
+#endif
+    usb_vcp_send_strn(str, len);
+}
+
 int stdin_rx_chr(void) {
     for (;;) {
 #if 0
@@ -77,36 +87,49 @@ static const char *readline_hist[READLINE_HIST_SIZE] = {NULL, NULL, NULL, NULL,
 
 int readline(vstr_t *line, const char *prompt) {
     stdout_tx_str(prompt);
-    int len = vstr_len(line);
+    int orig_line_len = line->len;
     int escape_seq = 0;
-    int hist_num = 0;
+    int hist_cur = -1;
+    int cursor_pos = orig_line_len;
     for (;;) {
         int c = stdin_rx_chr();
+        int last_line_len = line->len;
+        int redraw_step_back = 0;
+        bool redraw_from_cursor = false;
+        int redraw_step_forward = 0;
         if (escape_seq == 0) {
-            if (VCP_CHAR_CTRL_A <= c && c <= VCP_CHAR_CTRL_D && vstr_len(line) == len) {
+            if (VCP_CHAR_CTRL_A <= c && c <= VCP_CHAR_CTRL_D && vstr_len(line) == orig_line_len) {
                 // control character with empty line
                 return c;
             } else if (c == '\r') {
                 // newline
                 stdout_tx_str("\r\n");
-                for (int i = READLINE_HIST_SIZE - 1; i > 0; i--) {
-                    readline_hist[i] = readline_hist[i - 1];
+                if (line->len > orig_line_len && (readline_hist[0] == NULL || strcmp(readline_hist[0], line->buf + orig_line_len) != 0)) {
+                    // a line which is not empty and different from the last one
+                    // so update the history
+                    for (int i = READLINE_HIST_SIZE - 1; i > 0; i--) {
+                        readline_hist[i] = readline_hist[i - 1];
+                    }
+                    readline_hist[0] = str_dup(line->buf + orig_line_len);
                 }
-                readline_hist[0] = str_dup(vstr_str(line));
                 return 0;
             } else if (c == 27) {
                 // escape sequence
                 escape_seq = 1;
             } else if (c == 127) {
                 // backspace
-                if (vstr_len(line) > len) {
-                    vstr_cut_tail(line, 1);
-                    stdout_tx_str("\b \b");
+                if (cursor_pos > orig_line_len) {
+                    vstr_cut_out_bytes(line, cursor_pos - 1, 1);
+                    // set redraw parameters
+                    redraw_step_back = 1;
+                    redraw_from_cursor = true;
                 }
             } else if (32 <= c && c <= 126) {
                 // printable character
-                vstr_add_char(line, c);
-                stdout_tx_str(line->buf + line->len - 1);
+                vstr_ins_char(line, cursor_pos, c);
+                // set redraw parameters
+                redraw_from_cursor = true;
+                redraw_step_forward = 1;
             }
         } else if (escape_seq == 1) {
             if (c == '[') {
@@ -118,23 +141,78 @@ int readline(vstr_t *line, const char *prompt) {
             escape_seq = 0;
             if (c == 'A') {
                 // up arrow
-                if (hist_num < READLINE_HIST_SIZE && readline_hist[hist_num] != NULL) {
-                    // erase line
-                    for (int i = line->len - len; i > 0; i--) {
-                        stdout_tx_str("\b \b");
-                    }
-                    // set line to history
-                    line->len = len;
-                    vstr_add_str(line, readline_hist[hist_num]);
-                    // draw line
-                    stdout_tx_str(readline_hist[hist_num]);
+                if (hist_cur + 1 < READLINE_HIST_SIZE && readline_hist[hist_cur + 1] != NULL) {
                     // increase hist num
-                    hist_num += 1;
+                    hist_cur += 1;
+                    // set line to history
+                    line->len = orig_line_len;
+                    vstr_add_str(line, readline_hist[hist_cur]);
+                    // set redraw parameters
+                    redraw_step_back = cursor_pos - orig_line_len;
+                    redraw_from_cursor = true;
+                    redraw_step_forward = line->len - orig_line_len;
+                }
+            } else if (c == 'B') {
+                // down arrow
+                if (hist_cur >= 0) {
+                    // decrease hist num
+                    hist_cur -= 1;
+                    // set line to history
+                    vstr_cut_tail_bytes(line, line->len - orig_line_len);
+                    if (hist_cur >= 0) {
+                        vstr_add_str(line, readline_hist[hist_cur]);
+                    }
+                    // set redraw parameters
+                    redraw_step_back = cursor_pos - orig_line_len;
+                    redraw_from_cursor = true;
+                    redraw_step_forward = line->len - orig_line_len;
+                }
+            } else if (c == 'C') {
+                // right arrow
+                if (cursor_pos < line->len) {
+                    redraw_step_forward = 1;
+                }
+            } else if (c == 'D') {
+                // left arrow
+                if (cursor_pos > orig_line_len) {
+                    redraw_step_back = 1;
                 }
             }
         } else {
             escape_seq = 0;
         }
+
+        // redraw command prompt, efficiently
+        if (redraw_step_back > 0) {
+            for (int i = 0; i < redraw_step_back; i++) {
+                stdout_tx_str("\b");
+            }
+            cursor_pos -= redraw_step_back;
+        }
+        if (redraw_from_cursor) {
+            if (line->len < last_line_len) {
+                // erase old chars
+                for (int i = cursor_pos; i < last_line_len; i++) {
+                    stdout_tx_str(" ");
+                }
+                // step back
+                for (int i = cursor_pos; i < last_line_len; i++) {
+                    stdout_tx_str("\b");
+                }
+            }
+            // draw new chars
+            stdout_tx_strn(line->buf + cursor_pos, line->len - cursor_pos);
+            // move cursor forward if needed (already moved forward by length of line, so move it back)
+            for (int i = cursor_pos + redraw_step_forward; i < line->len; i++) {
+                stdout_tx_str("\b");
+            }
+            cursor_pos += redraw_step_forward;
+        } else if (redraw_step_forward > 0) {
+            // draw over old chars to move cursor forwards
+            stdout_tx_strn(line->buf + cursor_pos, redraw_step_forward);
+            cursor_pos += redraw_step_forward;
+        }
+
         HAL_Delay(1);
     }
 }
diff --git a/stmhal/usart.c b/stmhal/usart.c
index 16b23240a8..d54c97be55 100644
--- a/stmhal/usart.c
+++ b/stmhal/usart.c
@@ -159,13 +159,13 @@ void usart_tx_str(pyb_usart_t usart_id, const char *str) {
     }
 }
 
-void usart_tx_bytes(pyb_usart_t usart_id, const char *data, uint len) {
-    for (; len > 0; data++, len--) {
-        usart_tx_char(usart_id, *data);
+void usart_tx_strn(pyb_usart_t usart_id, const char *str, uint len) {
+    for (; len > 0; str++, len--) {
+        usart_tx_char(usart_id, *str);
     }
 }
 
-void usart_tx_strn_cooked(pyb_usart_t usart_id, const char *str, int len) {
+void usart_tx_strn_cooked(pyb_usart_t usart_id, const char *str, uint len) {
     for (const char *top = str + len; str < top; str++) {
         if (*str == '\n') {
             usart_tx_char(usart_id, '\r');
@@ -219,7 +219,7 @@ static mp_obj_t usart_obj_tx_str(mp_obj_t self_in, mp_obj_t s) {
         if (MP_OBJ_IS_STR(s)) {
             uint len;
             const char *data = mp_obj_str_get_data(s, &len);
-            usart_tx_bytes(self->usart_id, data, len);
+            usart_tx_strn(self->usart_id, data, len);
         }
     }
     return mp_const_none;
diff --git a/stmhal/usart.h b/stmhal/usart.h
index 2b1b60bfda..acb6762d73 100644
--- a/stmhal/usart.h
+++ b/stmhal/usart.h
@@ -18,7 +18,8 @@ void usart_init(pyb_usart_t usart_id, uint32_t baudrate);
 bool usart_rx_any(pyb_usart_t usart_id);
 int usart_rx_char(pyb_usart_t usart_id);
 void usart_tx_str(pyb_usart_t usart_id, const char *str);
-void usart_tx_strn_cooked(pyb_usart_t usart_id, const char *str, int len);
+void usart_tx_strn(pyb_usart_t usart_id, const char *str, uint len);
+void usart_tx_strn_cooked(pyb_usart_t usart_id, const char *str, uint len);
 
 #if 0
 MP_DECLARE_CONST_FUN_OBJ(pyb_Usart_obj);