/*
   ==============================================
   Functions needed for manipulating the displays
   ==============================================
*/


void initDisplay();
void updateDisplay();
char* scrolltext(const char* text, unsigned int tick, int chars);
char* scrolltext(const char* text, unsigned int tick);

#define LCD 1 // Use the LCD instead of OLED

#if LCD == 1
// Use LCD
  // include the library code:
  #include <LiquidCrystal.h>
  
  const int rs = 12, en = 11, d4 = 5, d5 = 4, d6 = 3, d7 = 2;
  LiquidCrystal display(rs, en, d4, d5, d6, d7);
  
  #define DISPLAY_COLS 16
  #define DISPLAY_ROWS 2

#else
// OLED display
  #include <SPI.h>
  #include <Wire.h>
  #include <Adafruit_GFX.h>
  #include <Adafruit_SSD1306.h>
  
  //Functionality
  const int rs = 4;
  Adafruit_SSD1306 display(rs);
  
  #define NUMFLAKES 10
  #define XPOS 0
  #define YPOS 1
  #define DELTAY 2
  
  #define DISPLAY_COLS 21
  #define DISPLAY_ROWS 4
  
  
  #define LOGO16_GLCD_HEIGHT 16 
  #define LOGO16_GLCD_WIDTH  16 
  static const unsigned char PROGMEM logo16_glcd_bmp[] =
  { B00000000, B11000000,
    B00000001, B11000000,
    B00000001, B11000000,
    B00000011, B11100000,
    B11110011, B11100000,
    B11111110, B11111000,
    B01111110, B11111111,
    B00110011, B10011111,
    B00011111, B11111100,
    B00001101, B01110000,
    B00011011, B10100000,
    B00111111, B11100000,
    B00111111, B11110000,
    B01111100, B11110000,
    B01110000, B01110000,
    B00000000, B00110000 };
  
  #if (SSD1306_LCDHEIGHT != 32)
  #error("Height incorrect, please fix Adafruit_SSD1306.h!");
  #endif
#endif


void initDisplay() {
  // Initialise the Display
  display.begin(DISPLAY_COLS, DISPLAY_ROWS);
  display.noAutoscroll();
  display.clear();
}
void updateDisplay( const unsigned int tick ) {
  
  // Update the Display
  display.clear();
  display.setCursor(5, 0);
  display.print("|");
  
  display.setCursor(0, 0);
  display_scrolltext(line_buffer[line_buffer_read], tick , 5);


  display.setCursor(0, DISPLAY_ROWS);
  display.print("LPS:");
  display.print( lps );
  
  display.setCursor(10, DISPLAY_ROWS);
  display.print("T:");
  display.print( tick );
}

void display_scrolltext(const char* text, const unsigned int tick) {
  return display_scrolltext(text, tick, DISPLAY_COLS);
}
void display_scrolltext(const char* text, const unsigned int tick, const int nchars) {
    int len = strlen(text);

    char new_text[nchars];
    
    // text fits in nchars characters
    if ( len <= nchars )
    {
      for( int i = 0; i < len; i++){
        new_text[i] = text[i];
      }
      for (int i = len; i < nchars; i++ ) {
        new_text[i] = ' ';
      }
      display.print(new_text);
      return;
    }
 
    
    const char separator[] = "-";
    const unsigned int scroll_speed = 1;
    const int sep_len = strlen(separator);
    
    int i = 0;
    int offset = (scroll_speed * tick) % (len + sep_len);

    // copy string from offset until the end, or until nchars have been reached
    for ( i = 0; i < nchars && i < len - offset; i++ ) {
      new_text[i] = text[offset+i];
    }
    Serial.println(i);

    // pad with separator;
    for (int j = 0 ; i < nchars && j < sep_len ; j++ ) {
      
      new_text[i++] = separator[j];
    }
    Serial.println(i);

     
    // copy from beginning until nchars reached
    for ( ; i < nchars; i++ ) {
      new_text[i] = text[i - sep_len - (len - offset) ];
    }
    Serial.println(i);
    // Somewhere this null byte is rewritten
    new_text[nchars] = '\0';
    display.print(new_text);
    Serial.println(new_text);
}
 
/*
ABCDE
-+ +-

chars = 4; // length of output
len = 6 // length of string
offset = 3 // offset of output

==> 'DE - AB'
    int newlen = len + strlen(separator);
    char newtext[newlen];

    for ( int i = 0; i < len; i++ ) {
      newtext[i] = text[i];
    }
    for ( int i = 0; i < strlen(separator); i++ ) {
      newtext[len + i] = separator[i];
    }
    
    // text does not fit in chars characters
    // TODO: this works ugly, I want a separator in between start and end
    for ( int i = 0; i < chars; i ++ ) {
      display.print(newtext[(offset + i) % (newlen)]);
    }
}
*/