#include <PicoDVI.h> // Core display & graphics library

//both constructors change the PicoDVI object, so we override later before begin()
//16-bit colour
DVIGFX16 displayCol(DVI_RES_320x240p60, pico_sock_cfg); //no double buffer option
//monochrome hi-res display:
DVIGFX1 displayBW(DVI_RES_640x240p60,true, pico_sock_cfg); //false= no double buffer

//local font data, doesn't like references to font data in RAM in PicoDVI library
//8x8 font for Monochrome
//6x12 font for colour
#include "font.h"

typedef enum {
    DISP_COLOUR=1,
    DISP_BW
} dispMode_t;

typedef struct {    //things that are to be saved in EEPROM/FLASH
  dispMode_t dispMode;
  int cols;
  int rows;
  char foreColour;  //indexed into termColours
  char backColour;
  char debugOn;     
} nvSettings_t;

const nvSettings_t defSettings[4]={
  {.dispMode=DISP_BW,.cols=80,.rows=24,.foreColour=7,.backColour=0,.debugOn=0}, //BW mode for Picomite etc
  {.dispMode=DISP_BW,.cols=80,.rows=24,.foreColour=0,.backColour=7,.debugOn=0}, //BW mode inverted for Picomite etc
  {.dispMode=DISP_COLOUR,.cols=53,.rows=20,.foreColour=7,.backColour=0,.debugOn=0}, //colour mode for Picomite etc
  {.dispMode=DISP_COLOUR,.cols=53,.rows=20,.foreColour=0,.backColour=7,.debugOn=0}  //colour mode inverted for Picomite etc
};

nvSettings_t loadedSettings[4]; //full array to move to/from flash
nvSettings_t cur;
int jpValue=0;

//emulated EEPROM in flash for storing settings
#include <EEPROM.h>
//use flash block size
#define EEPROM_SIZE_FROM_FLASH 4096
//avoid location 0
#define EEPROM_DATA_START 1
bool eepromError=false;
bool statusDone=false;

//these are the character memory sizes, not necessarily the display size
#define X_SIZE (128)
#define Y_SIZE (32)
#define CHAR_MEM_SIZE (X_SIZE*Y_SIZE)
//memory
char dispMem[Y_SIZE][X_SIZE];
char* dispPtr=dispMem[0];     //1d array to access in bulk
uint8_t attMem[Y_SIZE][X_SIZE];
uint8_t* attPtr=attMem[0];     //1d array to access in bulk
uint8_t fgcMem[Y_SIZE][X_SIZE];
uint8_t* fgcPtr=fgcMem[0];     //1d array to access in bulk
uint8_t bgcMem[Y_SIZE][X_SIZE];
uint8_t* bgcPtr=bgcMem[0];     //1d array to access in bulk
#define ATTR_UNDERSCORE (4)
#define ATTR_REVERSE (7)
#define BITMAP_UNDERSCORE (512)
#define DEFAULT_FGC (7)
#define DEFAULT_BGC (0)
//active display terminal area
int xSize=50;
int ySize=18;
int topBlank=0;   //these are chars for mono mode and pixels for colour mode
int leftBlank=0;
//display/cursor update flags
char uFlag=0;
unsigned long tmOut=0;
#define CURSOR_PERIOD (800)
int xCursor=0,yCursor=0;    //these are indexed to dispMem
//output display buffer characteristics
uint16_t* bufferPtrCol;
uint8_t* bufferPtrBW;
//these in pixels
int bufferSize=0;
int bufferWidth=0;
int bufferHeight=0;
//these in characters
int bufferXchars=0;
int bufferYchars=0;
uint8_t curAttrib=0;
uint8_t curFGC=DEFAULT_FGC;
uint8_t curBGC=DEFAULT_BGC; //in mono mode, if FGC is black, display is black on white, otherwise, white on black
int lineToUpdate=0;       //moving from dispMem to active display
uint8_t bwFill=0;
char enableDebug=0;

#define STATUS_LED 9

//hardware for comms with MOD2
//serial port depends on proper UART pins
#define UART Serial1
#define UART_TX (0)
#define UART_RX (1)
#define UART_FIFO (4095)
#define UART_BAUD (115200)

//VT100/ANSI Escape sequence processing
#define ESC_CODE (0x1B)
#define ESC_BUFFER_SIZE 32
#define BLANK_CHAR (0)
char escBuffer[ESC_BUFFER_SIZE]="";
int escRx=0;       //in the middle of an ESCAPE sequence=length so far
#define ESC_PARM_COUNT (16)
int escParms[ESC_PARM_COUNT];

#define JP3_SHUNT (8)
#define JP4_SHUNT (7)
#define HPD_DETECT (27)
//need to do this analog as some devices only output 3.3V
#define HPD_THRESHOLD (256)
#define PICO_ONBOARD_LED (25)

void setup() {
  bool dispOK;
  int i,d;
  Serial.begin(115200);
  pinMode(STATUS_LED,OUTPUT);
  digitalWrite(STATUS_LED,LOW);
  pinMode(PICO_ONBOARD_LED,OUTPUT);
  digitalWrite(PICO_ONBOARD_LED,HIGH);  //show power on
  pinMode(JP3_SHUNT,INPUT_PULLUP);
  pinMode(JP4_SHUNT,INPUT_PULLUP);
  pinMode(HPD_DETECT,INPUT);
  UART.setRX(UART_RX);  //be ready for data
  UART.setTX(UART_TX);
  UART.setFIFOSize(UART_FIFO);
  UART.begin(UART_BAUD);  
  delay(10);
  jpValue=getJPvalue();
  if(jpValue<0){jpValue=0;}
  if(jpValue>3){jpValue=0;}
  EEPROM.begin(EEPROM_SIZE_FROM_FLASH);
  EEPROM.get(EEPROM_DATA_START,loadedSettings);
  cur=loadedSettings[jpValue];
  if((cur.rows>Y_SIZE)||(cur.cols>X_SIZE)||(cur.rows<0)||(cur.cols<0)||(cur.foreColour>7)||(cur.foreColour<0)||(cur.backColour>7)||(cur.backColour<0)){
    cur=defSettings[jpValue];
    eepromError=true;
  }  
  xSize=cur.cols;
  ySize=cur.rows;
  curFGC=cur.foreColour;
  curBGC=cur.backColour;
  enableDebug=cur.debugOn;
  if(cur.dispMode==DISP_COLOUR){
    //override mono settings
    //dvi_vertical_repeat = dvispec[DVI_RES_320x240p60].v_rep;
    dvi_vertical_repeat = 2;  //same value as above
    dvi_monochrome_tmds = false;        
    dispOK=displayCol.begin();
    bufferWidth=displayCol.width();
    bufferHeight=displayCol.height();
    topBlank=(bufferHeight-cur.rows*SMALL_FONT_HEIGHT)/2; //centre on visible area
    if(topBlank<0){topBlank=0;}
    leftBlank=(bufferWidth-cur.cols*SMALL_FONT_WIDTH)/2;
    if(leftBlank<0){leftBlank=0;}
  }else{
    dispOK=displayBW.begin();
    bufferWidth=displayBW.width();
    bufferHeight=displayBW.height();    
    topBlank=0;
    leftBlank=0;
  }
  if(dispOK==false) {
    digitalWrite(STATUS_LED,LOW);
    while(!Serial){
      digitalWrite(STATUS_LED,LOW);
      delay(100);
      digitalWrite(STATUS_LED,HIGH);
      delay(100);
    }
    Serial.println("Could not start display");
    showStatus();
    while(1){
      digitalWrite(STATUS_LED,LOW);
      delay(100);
      digitalWrite(STATUS_LED,HIGH);
      delay(100);
      d=getJPvalue();
      if(d!=jpValue){rp2040.reboot();}
      checkMenu();
    }
  }
  //delay(2000);
  if(cur.dispMode==DISP_COLOUR){
    bufferPtrCol=displayCol.getBuffer();
    bufferXchars=bufferWidth/SMALL_FONT_WIDTH;
    bufferYchars=bufferHeight/SMALL_FONT_HEIGHT;
    bufferSize=bufferWidth*bufferHeight;  //1 word per pixel
    for(i=0;i<bufferSize;i++){bufferPtrCol[i]=COLOUR_GREY;}   //a background/border
  }else{
    if(cur.foreColour==0){bwFill=0xFF;} //inverted
    bufferPtrBW=displayBW.getBuffer();
    bufferXchars=bufferWidth/8;
    bufferYchars=bufferHeight/8;
    bufferSize=bufferWidth*bufferHeight/8;  //1 bit per pixel
    for(i=0;i<bufferSize;i++){bufferPtrBW[i]=bwFill;}   //a background/border
  }
  for(i=0;i<CHAR_MEM_SIZE;i++){
    dispPtr[i]=BLANK_CHAR;
    attPtr[i]=curAttrib;
    fgcPtr[i]=curFGC;
    bgcPtr[i]=curBGC;
  }        
}

void loop() {
  int d;
  if(statusDone==false){    //show status once on boot once serial is available
    if(Serial){
      statusDone=true;
      showStatus();
    }
  }  
  d=getJPvalue();
  if(d!=jpValue){
    Serial.println("Jumper changed, rebooting.");
    delay(500);
    rp2040.reboot();
  }
  if(analogRead(HPD_DETECT)>HPD_THRESHOLD){
    digitalWrite(STATUS_LED,HIGH);
  }else{
    digitalWrite(STATUS_LED,LOW);
  }
  checkMenu();
  if(UART.available()){
    while(UART.available()&&(yCursor<ySize)){ //allow update if scrolling needed
      d=UART.read();
      if(d==ESC_CODE){
        escBuffer[0]=ESC_CODE;
        escRx=1;
      }else{
        if(escRx){
          escBuffer[escRx]=d;
          escRx++;
          if((d>=0x40)&&(d<=0x7E)){
            if((escRx!=2)||(d!='[')){ //allow sequence to continue after ESC [
              processEsc();
              escRx=0;
            }
          }
        }else{
          //display.write(d);
          switch(d){
            case 8: //backspace
              xCursor=xCursor-1;
              if(xCursor<0){xCursor=0;}
              break;
            case 9: //tab
              xCursor=(xCursor+8)&0xF8;
              if(xCursor>=xSize){
                xCursor=0;
                yCursor=yCursor+1;
              }
              break;
            case 10:  //line feed
              yCursor=yCursor+1;
              if(enableDebug){Serial.println("<LF>");}
              break;
            case 12:  //form feed
              yCursor=yCursor+1; // this mimics what Micromite does
              if(enableDebug){Serial.println("<FF>");}
              break;
            case 13:  //carriage return
              xCursor=0;
              if(enableDebug){Serial.println("<CR>");}
              break;
            default:
              if(d>31){
                dispMem[yCursor][xCursor]=d;
                attMem[yCursor][xCursor]=curAttrib;
                fgcMem[yCursor][xCursor]=curFGC;
                bgcMem[yCursor][xCursor]=curBGC;
                xCursor=xCursor+1;
                if(xCursor>=xSize){
                  xCursor=0;
                  yCursor=yCursor+1;
                }
                if(enableDebug){Serial.write(d);}
              }
              break;
          }          
        }
      }
    }   
  }  
  //handle scroll/wrap/overflow
  if(yCursor>=ySize){
    yCursor=ySize-1;
    scrollUpOne();
  }
  if(cur.dispMode==DISP_COLOUR){
    updateLine16();
  }else{
    updateLineBW();
  }  
}


void processEsc(void){
  int x,y,i;
  getEscParms();
  if(enableDebug){
    Serial.printf("ESC %d bytes\r\n",escRx);
    Serial.write(escBuffer,escRx);
    Serial.printf("\r\n 1:%d 2:%d 3:%d\r\n",escParms[0],escParms[1],escParms[2]);
    Serial.println();
  }
  if(escBuffer[1]=='['){
    switch(escBuffer[escRx-1]){ //last character specifies function
      case 'A':   //CUP cursor up
        if(escParms[0]){
          yCursor=yCursor-escParms[0];
        }else{
          yCursor=yCursor-1;
        }
        if(yCursor<0){
          yCursor=0;
          //scrollDownOne();
        } 
        break;
      case 'B':   //CUD cursor down
        if(escParms[0]){
          yCursor=yCursor+escParms[0];
        }else{
          yCursor=yCursor+1;
        }
        if(yCursor>=ySize){
          yCursor=ySize-1;
          //scrollUpOne();
        }
        break;
      case 'C':   //CUF cursor forward
        if(escParms[0]){
          xCursor=xCursor+escParms[0];
        }else{
          xCursor=xCursor+1;
        }
        if(xCursor>=xSize){xCursor=xSize-1;}
        break;
      case 'D':   //CUB cursor back
        if(escParms[0]){
          xCursor=xCursor-escParms[0];
        }else{
          xCursor=xCursor-1;
        }
        if(xCursor<0){xCursor=0;}
        break;
      case 'E':   //CNL cursor next line down
        xCursor=0;
        if(escParms[0]){
          yCursor=yCursor+escParms[0];
        }else{
          yCursor=yCursor+1;
        }
        if(yCursor>=ySize){
          yCursor=ySize-1;
          scrollUpOne();
        }
        break;
      case 'F':   //CPL cursor next line up
        xCursor=0;
        if(escParms[0]){
          yCursor=yCursor-escParms[0];
        }else{
          yCursor=yCursor-1;
        }
        if(yCursor<0){
          yCursor=0;
          scrollDownOne();
        }
        break;
      case 'G':  //CHA cursor horizontal absolute
        if(escParms[0]){
          xCursor=escParms[0];
        }else{
          xCursor=1;
        }
        if(xCursor<0){xCursor=0;}   //shouldn't, but worth checking
        if(xCursor>=xSize){xCursor=xSize-1;}
        break;
      case 'f':   //according to Micromite VT100 specs
      case 'H':   //CUP cursor position
        if(escParms[1]){
          xCursor=escParms[1]-1;  //1-based, row, column order
        }else{
          xCursor=0;
        }        
        if(escParms[0]){
          yCursor=escParms[0]-1;  //1-based
        }else{
          yCursor=0; //top of current page
        }        
        if(xCursor<0){xCursor=0;}
        if(xCursor>=xSize){xCursor=xSize-1;}
        if(yCursor<0){yCursor=0;}           
        if(yCursor>=ySize){yCursor=ySize-1;}  //clip, don't scroll
        break;      
      case 'J': //erase part of screen
        switch(escParms[0]){
          case 0: //erase from cursor to end
            for(i=xCursor+yCursor*X_SIZE;i<CHAR_MEM_SIZE;i++){
              dispPtr[i]=BLANK_CHAR;
              attPtr[i]=curAttrib;
              fgcPtr[i]=curFGC;
              bgcPtr[i]=curBGC;
            }
            break;
          case 1: //erase from start to cursor
            for(i=0;i<=(xCursor+yCursor*X_SIZE);i++){
              dispPtr[i]=BLANK_CHAR;
              attPtr[i]=curAttrib;
              fgcPtr[i]=curFGC;
              bgcPtr[i]=curBGC;
            }
            break;
          case 2: //erase whole screen
            for(i=0;i<CHAR_MEM_SIZE;i++){
              dispPtr[i]=BLANK_CHAR;
              attPtr[i]=curAttrib;
              fgcPtr[i]=curFGC;
              bgcPtr[i]=curBGC;
            }      
            break;
          default: break; //ignore others
        }
        break;
      case 'K': //erase part of line
        switch(escParms[0]){
          case 0: //erase from cursor to end
            for(i=xCursor;i<X_SIZE;i++){
              dispMem[yCursor][i]=BLANK_CHAR;
              attMem[yCursor][i]=curAttrib;
              fgcMem[yCursor][i]=curFGC;
              bgcMem[yCursor][i]=curBGC;
            }      
            break;
          case 1: //erase from start  to cursor
            for(i=0;i<=xCursor;i++){
              dispMem[yCursor][i]=BLANK_CHAR;
              attMem[yCursor][i]=curAttrib;
              fgcMem[yCursor][i]=curFGC;
              bgcMem[yCursor][i]=curBGC;
            }      
            break;
          case 2: //erase whole line
            for(i=0;i<X_SIZE;i++){
              dispMem[yCursor][i]=BLANK_CHAR;
              attMem[yCursor][i]=curAttrib;
              fgcMem[yCursor][i]=curFGC;
              bgcMem[yCursor][i]=curBGC;
            }      
            break;
          default: break; //ignore others
        }
        break;
      case 'm':
        if(escParms[0]<8){
          curAttrib=escParms[0];
          if(escParms[0]==0){
            curFGC=cur.foreColour;
            curBGC=cur.backColour;
          }
        }else if((escParms[0]>=30)&&(escParms[0]<=37)){
          curFGC=escParms[0]-30;
        }else if((escParms[0]>=40)&&(escParms[0]<=47)){
          curBGC=escParms[0]-40;
        }        
        break;
      case 'l':
        if(enableDebug){Serial.printf("ESC[%dl not supported\r\n",escParms[0]);}
        break;
      default:
        if(enableDebug){
          Serial.printf("ESC %c not handled\r\n",escBuffer[escRx-1]);
          Serial.printf("ESC %d bytes\r\n",escRx);
          Serial.write(escBuffer,escRx);
          Serial.printf("\r\n 1:%d 2:%d 3:%d\r\n",escParms[0],escParms[1],escParms[2]);
          Serial.println();
        }
        break;
    }
  }else{
    switch(escBuffer[escRx-1]){ //last character specifies function; in this case it will be second character
      case 'D':
        scrollUpOne();
        break;
      case 'M':
        scrollDownOne();
        break;
      default:
        break;
    }
  }
}

void getEscParms(void){
  int i;
  int p=2;  //assuming we have standard CSI starting ESC [
  for(i=0;i<ESC_PARM_COUNT;i++){escParms[i]=0;}
  i=0;
  while((p<escRx)&&(i<ESC_PARM_COUNT)){
    if((escBuffer[p]>='0')&&(escBuffer[p]<='9')){
      escParms[i]=escParms[i]*10+escBuffer[p]-'0';      
    }else if(escBuffer[p]==';'){
      i=i+1;    //next field
    }
    p=p+1;
  }
}

//16bit colour renderer
void updateLine16(void){   //render a single line from dispMem to active display
  int x,y,b,xMax,p=0;
  uint16_t bMask=1;
  uint16_t cMask=0;
  uint16_t cMaskOn=0;
  uint16_t aMask=0;   //attribute mask
  uint16_t fc,bc; //foreground, background colour
  if(xSize>bufferXchars){
    xMax=bufferXchars;
  }else{
    xMax=xSize;
  }
  if((millis()/(CURSOR_PERIOD/2))&1){cMaskOn=0xFFFF;}   
  for(y=0;y<SMALL_FONT_HEIGHT;y++){
    p=(lineToUpdate*SMALL_FONT_HEIGHT+topBlank)*bufferWidth+y*bufferWidth+leftBlank;
    //p=((lineToUpdate+topBlank)*bufferWidth*SMALL_FONT_HEIGHT)+leftBlank*SMALL_FONT_WIDTH+y*bufferWidth;
    for(x=0;x<xMax;x++){
      if((x==xCursor)&&(lineToUpdate==yCursor)){      //handle cursor here
        cMask=cMaskOn;
      }else{
        cMask=0;
      }
      //get colour / attribs here
      fc=termColours[fgcMem[lineToUpdate][x]];
      bc=termColours[bgcMem[lineToUpdate][x]];
      switch(attMem[lineToUpdate][x]){
        case ATTR_REVERSE: aMask=0xFFFF; break;   //inverse
        case ATTR_UNDERSCORE: aMask=BITMAP_UNDERSCORE; break;  //underline
        default: aMask=0; break;
      }
      for(b=0;b<SMALL_FONT_WIDTH;b++){
        if(bMask&(font0612[dispMem[lineToUpdate][x]*SMALL_FONT_WIDTH+b]^aMask)){    //foreground
          bufferPtrCol[p]=fc^cMask;
        }else{            //background
          bufferPtrCol[p]=bc^cMask;
        }
        p++;
      }
    }
    bMask=bMask<<1;
  }
  lineToUpdate=(lineToUpdate+1)%(ySize);
}

//BW renderer
void updateLineBW(void){   //render a single line from dispMem to active display
  int x,y,b,xMax,p=0;
  uint8_t cMask=0;
  uint8_t cMaskOn=0;
  if(xSize>bufferXchars){
    xMax=bufferXchars;
  }else{
    xMax=xSize;
  }
  if((millis()/(CURSOR_PERIOD/2))&1){cMaskOn=255;}   
  y=lineToUpdate;
  for(b=0;b<2048;b=b+256){
    p=((lineToUpdate+topBlank)*bufferWidth)+(b*bufferWidth)/2048+leftBlank;
    for(x=0;x<xSize;x++){
      if((x==xCursor)&&(lineToUpdate==yCursor)){      //handle cursor here
        cMask=cMaskOn;
      }else{
        cMask=0;
      }
      cMask=cMask^bwFill;
      switch(attMem[y][x]){
        case ATTR_REVERSE:
          bufferPtrBW[p]=font_8x8[dispMem[y][x]+b]^0xFF^cMask;
          break;
        case ATTR_UNDERSCORE:
          if(b==1792){
              bufferPtrBW[p]=0xFF^cMask;
          }else{
              bufferPtrBW[p]=font_8x8[dispMem[y][x]+b]^cMask;
          }
          break;
        default:
          bufferPtrBW[p]=font_8x8[dispMem[y][x]+b]^cMask;
          break;
      }
      p++;
    }
  }
  lineToUpdate=(lineToUpdate+1)%(ySize);
  if(lineToUpdate==0){
    displayBW.swap(true);
    bufferPtrBW=displayBW.getBuffer();
  }
}

void scrollDownOne(void){   //ie trying to go above 1st line, everything moves down, first line cleared
  int i;
  for(i=CHAR_MEM_SIZE-X_SIZE-1;i>=0;i--){    //shift down
    dispPtr[i+X_SIZE]=dispPtr[i];
    attPtr[i+X_SIZE]=attPtr[i];
    fgcPtr[i+X_SIZE]=fgcPtr[i];
    bgcPtr[i+X_SIZE]=bgcPtr[i];
  }      
  for(i=0;i<X_SIZE;i++){    //blanks first line
    dispPtr[i]=BLANK_CHAR;    
    attPtr[i]=curAttrib;    
    fgcPtr[i]=curFGC;
    bgcPtr[i]=curBGC;
  }
  if(enableDebug){Serial.println("Scroll down");}
}

void scrollUpOne(void){   //ie trying to go below last line, everything moves up, last line is cleared
  int i;
  for(i=0;i<(CHAR_MEM_SIZE-X_SIZE);i++){    //shift up
    dispPtr[i]=dispPtr[i+X_SIZE];
    attPtr[i]=attPtr[i+X_SIZE];
    fgcPtr[i]=fgcPtr[i+X_SIZE];
    bgcPtr[i]=bgcPtr[i+X_SIZE];
  }      
  for(i=CHAR_MEM_SIZE-X_SIZE;i<CHAR_MEM_SIZE;i++){  //blank last
    dispPtr[i]=BLANK_CHAR;    
    attPtr[i]=curAttrib;    
    fgcPtr[i]=curFGC;
    bgcPtr[i]=curBGC;
  }
  if(enableDebug){Serial.println("Scroll up");}
}

int getJPvalue(void){
  int r=0;    //use JP off=0, which means reversed logic due to being digital high
  if(digitalRead(JP3_SHUNT)==LOW){r=r+1;}
  if(digitalRead(JP4_SHUNT)==LOW){r=r+2;}
  return r;
}

void showMenu(void){
  Serial.println("___________________________________");
  Serial.println("Setup Menu:");  
  Serial.printf("A/B: Colour/Mono (currently %s)\r\n",((cur.dispMode==DISP_COLOUR)?"COLOUR":"MONO"));
  Serial.printf("C/D: Columns/Rows (%dx%d)\r\n",cur.cols,cur.rows);
  Serial.printf("E/F: Colours (%s on %s)\r\n",cNames[cur.foreColour],cNames[cur.backColour]);
  Serial.printf("G:   Toggle debug (currently %s)\r\n",(enableDebug?"ON":"OFF"));
  Serial.println("S: Show display status");
  Serial.println("X: Reboot");
  Serial.println("Y: Save to flash");
  Serial.println("Z: Restore defaults");
}

void checkMenu(void){
  int d;
  static int n=0;   //for numerical entry
  static int s=0;   //menu selection
  if(Serial.available()){
    d=Serial.read();
    switch(d){
      case '~':
        showMenu();
        break;
      case 's':   //show status
      case 'S':
        showStatus();
        break;
      case 'x':   //reboot to reload settings
      case 'X':
        rp2040.reboot();
        break;
      case 'y':
      case 'Y':
        saveToFlash();
        showMenu();
        break;
      case 'z':
      case 'Z':
        loadDefaults();
        showMenu();
        break;
      case 'a':
      case 'A':
        cur.dispMode=DISP_COLOUR;        
        showMenu();
        break;        
      case 'b':
      case 'B':
        cur.dispMode=DISP_BW;
        if(cur.foreColour!=0){      //pick appropriate mono colour scheme
          cur.foreColour=7;cur.backColour=0;
        }else{
          cur.foreColour=0;cur.backColour=7;
        }
        showMenu();
        break;        
      case 'c':
      case 'C':
        Serial.println("Enter number of columns:");
        s='c';
        break;
      case 'd':
      case 'D':
        Serial.println("Enter number of rows:");
        s='d';
        break;
      case 'e':
      case 'E':
        Serial.println("Select foreground colour:");
        showColours();
        s='e';
        break;
      case 'f':
      case 'F':
        Serial.println("Select background colour:");
        showColours();
        s='f';
        break;
      case 'g':
      case 'G':
        if(enableDebug){
          enableDebug=0;
        }else{
          enableDebug=1;
        }
        cur.debugOn=enableDebug;
        Serial.println("Debug toggled");
        showMenu();
        break;
      default: break; //ignore
    }
    if(s!=0){
      if((d>='0')&&(d<='9')){
        n=n*10+d-'0';
        Serial.write(d);
      }
      if(d==8){ //backspace
        if(n){
          n=n/10;
          Serial.write(8);
        }
      }
      if(d==13){  //enter
        Serial.println();
        switch(s){
          case 'c':
            if((n>0)&&(n<X_SIZE)){
              cur.cols=n;
              s=0;
              n=0;
            }else{
              Serial.println("Out of range");
              s=0;
              n=0;
            }
            break;
          case 'd':
            if((n>0)&&(n<Y_SIZE)){
              cur.rows=n;
              s=0;
              n=0;
            }else{
              Serial.println("Out of range");
              s=0;
              n=0;
            }
            break;
          case 'e':
            if((n>0)&&(n<8)){
              cur.foreColour=n;
              s=0;
              n=0;
            }else{
              Serial.println("Out of range");
              s=0;
              n=0;
            }
            break;
          case 'f':
            if((n>=0)&&(n<8)){
              cur.backColour=n;
              s=0;
              n=0;
            }else{
              Serial.println("Out of range");
              s=0;
              n=0;
            }
            break;
          default:
            Serial.println("\r\nError");
            s=0;
            n=0;
            break;
        }
        showMenu();
      }
      if(d==27){
        Serial.println("\r\nCancelled");
        s=0;
        n=0;
      }
    }
  }
}

void showColours(void){
  int i;
  for(i=0;i<8;i++){
    Serial.printf("%d:%s\r\n",i,cNames[i]);
  }
}

void saveToFlash(void){
  loadedSettings[jpValue]=cur;
  EEPROM.put(EEPROM_DATA_START,loadedSettings);
  if(EEPROM.commit()){
    Serial.println("Saved to flash");
  }else{
    Serial.println("ERROR! Save to flash failed");
  }
}

void loadDefaults(void){
  Serial.println("Defaults loaded");
  cur=defSettings[jpValue];
}

void showStatus(void){  
  if(eepromError){
    Serial.println("EEPROM Error, defaults loaded");
  }
  Serial.printf("Jumper = %d: %s, %d x %d chars, %s on %s\r\n",jpValue,((cur.dispMode==DISP_COLOUR)?"COLOUR":"MONO"),cur.cols,cur.rows,cNames[cur.foreColour],cNames[cur.backColour]);
  Serial.printf("Display initialised at %d x %d pixels.\r\n",bufferWidth,bufferHeight);
  if(enableDebug){Serial.println("Debug is on");}else{Serial.println("Debug is off");}
  Serial.printf("JP3 is %s\r\n",(digitalRead(JP3_SHUNT)?"OUT":"IN"));
  Serial.printf("JP4 is %s\r\n",(digitalRead(JP4_SHUNT)?"OUT":"IN"));
  Serial.printf("HPD is %s\r\n",((analogRead(HPD_DETECT)>HPD_THRESHOLD)?"CONNECTED":"NOT CONNECTED"));
  Serial.println("Press ~ for setup menu");
}