From 5103ba1c23f0b266c070b34b16fc8bf5d2c1f560 Mon Sep 17 00:00:00 2001
From: Roman <bapstr@ethz.ch>
Date: Thu, 19 Nov 2015 09:09:51 +0100
Subject: [PATCH] implemented better shell for SITL

---
 src/platforms/posix/main.cpp | 80 ++++++++++++++++++++++++++++++++----
 1 file changed, 72 insertions(+), 8 deletions(-)

diff --git a/src/platforms/posix/main.cpp b/src/platforms/posix/main.cpp
index 1b22627721..149ae6c185 100644
--- a/src/platforms/posix/main.cpp
+++ b/src/platforms/posix/main.cpp
@@ -35,6 +35,7 @@
  * Basic shell to execute builtin "apps"
  *
  * @author Mark Charlebois <charlebm@gmail.com>
+ * @auther Roman Bapst <bapstroman@gmail.com>
  */
 
 #include <iostream>
@@ -43,6 +44,7 @@
 #include <sstream>
 #include <vector>
 #include <signal.h>
+#include <termios.h>
 
 namespace px4
 {
@@ -56,6 +58,8 @@ typedef int (*px4_main_t)(int argc, char *argv[]);
 #include "apps.h"
 #include "px4_middleware.h"
 
+#define CMD_BUFF_SIZE	100
+
 static bool _ExitFlag = false;
 extern "C" {
 	void _SigIntHandler(int sig_num);
@@ -203,22 +207,82 @@ int main(int argc, char **argv)
 		}
 
 		if (!daemon_mode) {
-			string mystr;
+			string mystr = "";
+			string string_buffer[CMD_BUFF_SIZE];
+			int buf_ptr_write = 0;
+			int buf_ptr_read = 0;
 
 			print_prompt();
 
+			// change input mode so that we can manage shell
+			struct termios term;
+			tcgetattr(0, &term);
+			term.c_lflag &= ~ICANON;
+			term.c_lflag &= ~ECHO;
+			tcsetattr(0, TCSANOW, &term);
+			setbuf(stdin, NULL);
+
 			while (!_ExitFlag) {
 
-				struct pollfd fds;
-				int ret;
-				fds.fd = 0; /* stdin */
-				fds.events = POLLIN;
-				ret = poll(&fds, 1, 100);
+				char c = getchar();
+
+				switch (c) {
+				case 127:	// backslash
+					if (mystr.length() > 0) {
+						mystr.pop_back();
+						printf("%c[2K", 27);	// clear line
+						cout << (char)13;
+						print_prompt();
+						cout << mystr;
+					}
+
+					break;
+
+				case'\n':	// user hit enter
+					if (buf_ptr_write == CMD_BUFF_SIZE) {
+						buf_ptr_write = 0;
+					}
 
-				if (ret > 0) {
-					getline(cin, mystr);
+					string_buffer[buf_ptr_write] = mystr;
+					buf_ptr_write++;
 					process_line(mystr, !daemon_mode);
 					mystr = "";
+					buf_ptr_read = buf_ptr_write;
+					break;
+
+				case '\033': {	// arrow keys
+						c = getchar();	// skip first one, does not have the info
+						c = getchar();
+
+						if (c == 'A') {
+							buf_ptr_read--;
+
+						} else if (c == 'B') {
+							if (buf_ptr_read < buf_ptr_write) {
+								buf_ptr_read++;
+							}
+
+						} else {
+							// TODO: Support editing current line
+						}
+
+						if (buf_ptr_read < 0) {
+							buf_ptr_read = 0;
+						}
+
+						string saved_cmd = string_buffer[buf_ptr_read];
+						printf("%c[2K", 27);
+						cout << (char)13;
+						mystr = saved_cmd;
+						print_prompt();
+						cout << mystr;
+						break;
+					}
+
+				default:	// any other input
+					cout << c;
+					mystr += c;
+					break;
 				}
 			}