//#include <SPI.h>
#include <Wire.h>
//#include <protothreads.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <string.h>

const String SLIDERCAM = "SliderCam 1.0";

#define pinInicio 2               // Pin del boton de INICIO/DETENER
#define pinCambio 3               // Pin indicador del cambio de dirección del slider
#define pinDireccion 7            // Pin indicador de direccion del slider
#define pinPasos 8                // Pin control de pasos del slider
#define pinParada 9               // Pin indicador del final de carrera del slider
#define pinVelocidad A0           // Pin del potenciómetro de velocidad del slider

#define SCREEN_WIDTH 128 // OLED display width, in pixels
#define SCREEN_HEIGHT 64 // OLED display height, in pixels

// Declaration for an SSD1306 display connected to I2C (SDA, SCL pins)
#define OLED_RESET     4 // Reset pin # (or -1 if sharing Arduino reset pin)
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);
//Adafruit_SSD1306 display(OLED_RESET);

#define LOGO_HEIGHT   50
#define LOGO_WIDTH    50
static const unsigned char PROGMEM logo_bmp[] =
{     // '4J, 52x52px
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x70, 0x00, 0x00, 
  0x00, 0x00, 0x00, 0x01, 0xc0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x07, 0x00, 0x00, 0x00, 0x00, 0x00, 
  0x01, 0xfc, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3f, 0x10, 0x00, 0x00, 0x00, 0x00, 0x01, 0xf0, 0x20, 
  0x00, 0x00, 0x00, 0x00, 0x03, 0xc0, 0x60, 0x00, 0x00, 0x00, 0x00, 0x0f, 0x00, 0xc0, 0x00, 0x00, 
  0x00, 0x00, 0x1e, 0x01, 0x80, 0x00, 0x00, 0x00, 0x00, 0x1c, 0x03, 0x00, 0x00, 0x00, 0x00, 0x1e, 
  0x38, 0x06, 0x00, 0x00, 0x00, 0x00, 0x60, 0x30, 0x0e, 0x00, 0x00, 0x00, 0x01, 0x80, 0x30, 0x1c, 
  0x00, 0x00, 0x00, 0x02, 0x80, 0x30, 0x1c, 0x20, 0x00, 0x00, 0x05, 0x00, 0x20, 0x38, 0x20, 0x00, 
  0x00, 0x09, 0x00, 0x20, 0x70, 0x20, 0x00, 0x00, 0x12, 0x00, 0x30, 0x70, 0x40, 0x00, 0x00, 0x26, 
  0x00, 0x10, 0xe0, 0xc0, 0x00, 0x00, 0x44, 0x00, 0x18, 0xe1, 0x80, 0x00, 0x00, 0x8c, 0x80, 0x07, 
  0xc2, 0x00, 0x00, 0x01, 0x18, 0x80, 0x03, 0xf8, 0x00, 0x00, 0x02, 0x18, 0x80, 0x03, 0x80, 0x00, 
  0x00, 0x08, 0x31, 0x00, 0x07, 0x80, 0x00, 0x00, 0x7f, 0xfe, 0x00, 0x07, 0x00, 0x00, 0x00, 0xc0, 
  0x60, 0x00, 0x0f, 0x00, 0x00, 0x00, 0x00, 0xe0, 0x00, 0x0e, 0x00, 0x00, 0x00, 0x01, 0xc0, 0x00, 
  0x1e, 0x00, 0x00, 0x00, 0x01, 0xc0, 0x00, 0x3c, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3c, 0x00, 
  0x00, 0x00, 0x00, 0x00, 0x00, 0x78, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x78, 0x00, 0x00, 0x00, 
  0x00, 0x80, 0x00, 0xf0, 0x00, 0x00, 0x00, 0x03, 0xc0, 0x01, 0xe0, 0x00, 0x00, 0x00, 0x03, 0xc0, 
  0x03, 0xc0, 0x00, 0x00, 0x00, 0x03, 0xc0, 0x03, 0x80, 0x00, 0x00, 0x00, 0x02, 0x00, 0x0f, 0x00, 
  0x00, 0x00, 0x00, 0x03, 0x00, 0x1c, 0x00, 0x00, 0x00, 0x00, 0x01, 0x80, 0x70, 0x00, 0x00, 0x00, 
  0x00, 0x00, 0x7f, 0xc0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3f, 
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x30, 0x7e, 0xff, 0xe5, 0x98, 0xf7, 0xc0, 0x1f, 0x62, 0xe1, 
  0x3f, 0x38, 0xf7, 0x80, 0x21, 0x62, 0xe1, 0x1b, 0x3c, 0xf6, 0x00, 0x3f, 0x7e, 0x81, 0x1a, 0x46, 
  0x97, 0xc0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00
};

//struct pt hiloSlider;
#define MAX_VEL 2
#define LONG_SLIDER 100           // 100cms 

int velocidad = 50;               // Velocidad del slider
int sentido = LOW;                // Sentido IZQUIERDA/DERECHA de movimiento del slider
int inicio = LOW;                 // Estado INICIO/DETENIDO del slider

long totalPasos = 1;              // Contador de pasos totales para el Slider 
long posActual = 0;               // Posición actual en pasos del carro en el Slider 
bool checking = false;            // Verificador del modo de chequeo

//DateTime fecha = DateTime(2020, 7, 10, 22, 22, 10);
float temp = 35.33445432135513;

void setup() {
  // Inicializar puerto serial
  Serial.begin(9600);

  //PT_INIT(&hiloSlider);

  //fecha = DateTime(2020, 7, 10, 22, 22, 10);

  // Pines de salida
  pinMode(pinPasos, OUTPUT);
  pinMode(pinDireccion, OUTPUT);

  // Pines de entrada
  pinMode(pinInicio, INPUT_PULLUP);
  pinMode(pinCambio, INPUT_PULLUP);
  pinMode(pinParada, INPUT_PULLUP);
  pinMode(pinVelocidad, INPUT);
  
  // Inicializar pantalla
  // SSD1306_SWITCHCAPVCC = generate display voltage from 3.3V internally
  if(!display.begin(SSD1306_SWITCHCAPVCC, 0x3D)) { // Address 0x3D for 128x64
    Serial.println(F("SSD1306 allocation failed"));
    for(;;); // Don't proceed, loop forever
  }

  // Pantalla de Bienvenida
  Bienvenida();

  // Revisar Configuracion
  InitialCheck();

  // Pantalla de Operacion
  PantallaOP();

  attachInterrupt(digitalPinToInterrupt(pinInicio), CambioEstado, FALLING);
  attachInterrupt(digitalPinToInterrupt(pinCambio), CambioDireccion, FALLING);
  attachInterrupt(digitalPinToInterrupt(pinParada), Parada, FALLING);
}


void PantallaOP(){
  // En espera
  display.setTextColor(SSD1306_INVERSE); 
  display.clearDisplay();
  display.drawLine(0, 8, display.width()-1, 8, SSD1306_WHITE);
  display.setCursor(0,0);
  //display.setTextColor(SSD1306_WHITE);
  display.print(F("VEL: 000 POS: 0000000"));
  display.setCursor(30,0);   //6xLen("VEL: ") 8px alto
  //display.setTextColor(SSD1306_BLACK);
  display.print(F("000"));
  display.setCursor(84,0);   //6xLen("VEL: 000  POS: ") 8px alto
  display.print(F("0000000"));
  
  //display.setTextColor(SSD1306_WHITE);
  display.setCursor(30,0);   //6xLen("VEL: ") 8px alto
  display.print(103-velocidad);
  display.setCursor(84,0);   //6xLen("VEL: 000  POS: ") 8px alto
  display.print(posActual*LONG_SLIDER/totalPasos);
  
  display.fillRect(posActual*(display.width()-10)/totalPasos, 45, 10, 10, SSD1306_INVERSE);
  display.drawLine(0, 55, display.width()-1, 55, SSD1306_WHITE);
  display.drawLine(0, 62, display.width()-1, 62, SSD1306_WHITE);
  display.drawLine(0, 63, display.width()-1, 63, SSD1306_WHITE);
  
  display.setTextColor(SSD1306_INVERSE);
  display.setCursor(0,24);
  display.println(F("En espera... "));
  display.display();
  Serial.println("En espera... ");
  
}

void loop() {
  Slider (nullptr);
  //Slider (&hiloSlider);
}

void Bienvenida(){
  display.clearDisplay();
  delay(1000);

  Serial.println(SLIDERCAM);

  // Mostrar Logo 4J
  display.setTextSize(1);                 // Normal 1:1 pixel scale
  display.setTextColor(SSD1306_WHITE);    // Draw white text
  display.setCursor(0,0);                 // Start at top-left corner
  display.println(SLIDERCAM);
  
  display.drawBitmap(
    (display.width()  - LOGO_WIDTH ) / 2,
    (display.height() - LOGO_HEIGHT) / 2,
    logo_bmp, LOGO_WIDTH, LOGO_HEIGHT, 1);
  
  display.setCursor(100,56);             // Start at bottom-right corner
  display.println(F("2021"));

  display.display();
  delay(1000);

  display.invertDisplay(true);
  delay(2000);
  display.invertDisplay(false);
  delay(1000);
}

void InitialCheck(){
  long t = 0;

  checking = true;

  display.clearDisplay();
  display.display();

  display.setCursor(10,0);
  display.println(F("Checking..."));
  display.display();

  if (sentido == LOW){
    Serial.print("<<<--- ");
  }
  else {
    Serial.print("--->>> ");
   }
  
  velocidad = analogRead(A0);
  velocidad = map(velocidad,0,1024,100,MAX_VEL);
  
  Serial.print("Velocidad inicial: ");
  Serial.println(103 - velocidad);

  // Ir al inicio del Slider
  EncontrarInicio();
  
  // Ir hasta el final del Slider
  EncontrarFinal();

  Reset();

  checking = false;
}

void EncontrarInicio(){
  long t = 0;
  sentido = LOW;
  display.setTextColor(SSD1306_INVERSE);    // Draw black text
  display.println(F("Buscando inicio... "));
  display.display();
  Serial.println("Buscando inicio... ");

  do{
    digitalWrite(pinPasos, HIGH);
    t = millis();
    while((millis()-t) < MAX_VEL);
        
    digitalWrite(pinPasos, LOW);
    t = millis();
    while((millis()-t) < MAX_VEL);

  } while (digitalRead(pinParada) == 1);

  t = millis();
  while((millis()-t) < 1000);

  digitalWrite(pinDireccion, !sentido);
  for (int i=0; i<2; i++){
    digitalWrite(pinPasos, HIGH);
    t = millis();
    while((millis()-t) < MAX_VEL);
        
    digitalWrite(pinPasos, LOW);
    t = millis();
    while((millis()-t) < MAX_VEL);
    
    if (sentido)
      posActual--;
    else
      posActual++;
  }
  
  display.setCursor(0,8);
  display.println(F("Buscando inicio... "));
  display.setCursor(0,8);
  display.println(F("Inicio encontrado"));
  display.display();
  Serial.println("Inicio encontrado");
}

void EncontrarFinal(){
  long t = 0;
  sentido = HIGH;
  display.setTextColor(SSD1306_INVERSE);    // Draw black text
  display.println(("Buscando final... "));
  display.display();
  Serial.println("Buscando final... ");

  totalPasos = 0;
  digitalWrite(pinDireccion, sentido);

  do{
    digitalWrite(pinPasos, HIGH);
    t = millis();
    while((millis()-t) < MAX_VEL);
        
    digitalWrite(pinPasos, LOW);
    t = millis();
    while((millis()-t) < MAX_VEL);

    totalPasos++;
  } while (digitalRead(pinParada));

  posActual = totalPasos;

  t = millis();
  while((millis()-t) < 1000);

  digitalWrite(pinDireccion, !sentido);
  for (int i=0; i<2; i++){
    digitalWrite(pinPasos, HIGH);
    t = millis();
    while((millis()-t) < MAX_VEL);
        
    digitalWrite(pinPasos, LOW);
    t = millis();
    while((millis()-t) < MAX_VEL);
    
    if (sentido)
      posActual--;
    else
      posActual++;
  }
  
  display.setCursor(0,16);
  display.println(F("Buscando final... "));
  display.setCursor(0,16);
  display.println(F("Final encontrado"));
  display.display();
  Serial.println("Final encontrado");
  Serial.print("Total de pasos: ");
  Serial.println(totalPasos);
}

void Reset(){
  long t = 0;
  sentido = LOW;
  digitalWrite(pinDireccion, sentido);
  display.println(F("Reiniciando... "));
  display.display();
  Serial.println(F("Reiniciando... "));

  Serial.print("posActual: ");
  Serial.print(posActual);
  Serial.print(" - totalPasos: ");
  Serial.println(totalPasos);
  for(posActual=totalPasos; posActual>0; posActual--){
    digitalWrite(pinPasos, HIGH);
    t = millis();
    while((millis()-t) < MAX_VEL);
        
    digitalWrite(pinPasos, LOW);
    t = millis();
    while((millis()-t) < MAX_VEL);

    if (!digitalRead(pinParada)) {
      digitalWrite(pinDireccion, !sentido);
      for (int i=0; i<2; i++){
        digitalWrite(pinPasos, HIGH);
        t = millis();
        while((millis()-t) < MAX_VEL);
            
        digitalWrite(pinPasos, LOW);
        t = millis();
        while((millis()-t) < MAX_VEL);

        if (sentido)
          posActual--;
        else
          posActual++;
      }
      break;
    }
  }
  
  inicio = LOW;
  sentido = HIGH;
  digitalWrite(pinDireccion, sentido);
  
  Serial.print("posActual: ");
  Serial.println(posActual);

  display.println(F("En Inicio"));
  display.display();
  Serial.println("En Inicio");
}

void CambioEstado(){
  inicio = !inicio;

  if (!inicio){
    display.fillRect(0, 24, display.width()-1, 8, SSD1306_BLACK);
    display.setCursor(0,24);
    display.print(F("Detenido"));
    display.fillRect((display.width()-18)/2, 32, 18, 8, SSD1306_BLACK);
    display.setCursor((display.width()-18)/2, 32);   //6xLen(">>>") 8px alto
    if (sentido) 
      display.print(F(">>>"));
    else
      display.print(F("<<<"));
  }
  else{
    display.fillRect(0, 24, display.width()-1, 8, SSD1306_BLACK);
    display.setCursor(0,24);
    display.print(F("Avanzando"));
  }
  Serial.print(F("Cambio Estado: "));
  Serial.println(inicio);
}

void CambioDireccion(){
  sentido = !sentido;
  display.fillRect((display.width()-18)/2, 32, 18, 8, SSD1306_BLACK);
//
//  if (sentido){
//    display.setCursor((display.width()-18)/2,32);   //6xLen(">>>") 8px alto
//    display.println(F(">>>"));
//  }
//  else{
//    display.setCursor((display.width()-18)/2,32);   //6xLen(">>>") 8px alto
//    display.println(F("<<<"));
//  }
//    display.display();
  

  if (sentido){
    Serial.println("--->>>");
  }
  else{
    Serial.println("<<<---");
  }
  digitalWrite(pinDireccion, sentido);
}

void Parada(){
  inicio = 0;
  display.fillRect(0, 24, display.width()-1, 8, SSD1306_BLACK);
  display.setCursor(0,24);
  display.print(F("Detenido"));
  display.fillRect((display.width()-18)/2, 32, 18, 8, SSD1306_BLACK);
  display.setCursor((display.width()-18)/2, 32);   //6xLen(">>>") 8px alto
  if (sentido) 
    display.print(F(">>>"));
  else
    display.print(F("<<<"));
  display.display();
  Serial.print(F("Detenido: "));
  Serial.println(inicio);
}

void Slider(struct pt *pt){
  //PT_BEGIN (pt);

  static long t = 0;
  
  if (inicio){
    
    if (!digitalRead(pinParada)){
      Parada();
      return;
    }
    
    display.setCursor(30,0);   //6xLen("VEL: ") 8px alto
    display.print(103-velocidad);
    display.setCursor(84,0);   //6xLen("VEL: 000  POS: ") 8px alto
    display.print(posActual*LONG_SLIDER/totalPasos);
    
    display.setCursor((display.width()-18)/2,32);   //6xLen(">>>") 8px alto
    if (sentido) 
      display.print(F(">>>"));
    else
      display.print(F("<<<"));
      
    velocidad = analogRead(A0);
    velocidad = map(velocidad,0,1024,100,MAX_VEL);
    
    //Serial.print("Velocidad: ");
    //Serial.print(velocidad);
    //Serial.print(" ");
    //Serial.println(103 - velocidad);
      
    display.fillRect(posActual*(display.width()-10)/totalPasos, 45, 10, 10, SSD1306_INVERSE);
    
    digitalWrite(pinPasos, HIGH);
    t = millis();
    while ((millis()-t) < velocidad);
    //PT_WAIT_WHILE(pt, (millis()-t) < velocidad);
      
    digitalWrite(pinPasos, LOW);
    t = millis();
    while ((millis()-t) < velocidad);
    //PT_WAIT_WHILE(pt, (millis()-t) < velocidad);

    if (sentido) 
      posActual++;
    else
      posActual--;

    if ((posActual<=0) || (posActual >= totalPasos)){
      inicio = LOW;
      sentido = !sentido;
      
      digitalWrite(pinDireccion, sentido);
      for (int i=0; i<2; i++){
        digitalWrite(pinPasos, HIGH);
        t = millis();
        while((millis()-t) < MAX_VEL);
            
        digitalWrite(pinPasos, LOW);
        t = millis();
        while((millis()-t) < MAX_VEL);

        if (sentido)
          posActual++;
        else
          posActual--;
      }
      
      display.fillRect(0, 24, display.width()-1, 8, SSD1306_BLACK);
      display.setCursor(0,24);   
      display.print(F("Detenido"));
      Serial.println("Detenido");
      
      display.fillRect((display.width()-18)/2, 32, 18, 8, SSD1306_BLACK);
      display.setCursor((display.width()-18)/2,32);   //6xLen(">>>") 8px alto
      if (sentido) 
        display.print(F(">>>"));
      else
        display.print(F("<<<"));
    }
    
    display.setCursor(30,0);   //6xLen("VEL: ") 8px alto
    display.print(103-velocidad);
    display.setCursor(84,0);   //6xLen("VEL: 000  POS: ") 8px alto
    display.print(posActual*LONG_SLIDER/totalPasos);
    display.fillRect(posActual*(display.width()-10)/totalPasos, 45, 10, 10, SSD1306_INVERSE);
    display.display();
  }

  //PT_END(pt);
}


/*
void testdrawline() {
  int16_t i;

  display.clearDisplay(); // Clear display buffer

  for(i=0; i<display.width(); i+=4) {
    display.drawLine(0, 0, i, display.height()-1, SSD1306_WHITE);
    display.display(); // Update screen with each newly-drawn line
    delay(1);
  }
  for(i=0; i<display.height(); i+=4) {
    display.drawLine(0, 0, display.width()-1, i, SSD1306_WHITE);
    display.display();
    delay(1);
  }
  delay(250);

  display.clearDisplay();

  for(i=0; i<display.width(); i+=4) {
    display.drawLine(0, display.height()-1, i, 0, SSD1306_WHITE);
    display.display();
    delay(1);
  }
  for(i=display.height()-1; i>=0; i-=4) {
    display.drawLine(0, display.height()-1, display.width()-1, i, SSD1306_WHITE);
    display.display();
    delay(1);
  }
  delay(250);

  display.clearDisplay();

  for(i=display.width()-1; i>=0; i-=4) {
    display.drawLine(display.width()-1, display.height()-1, i, 0, SSD1306_WHITE);
    display.display();
    delay(1);
  }
  for(i=display.height()-1; i>=0; i-=4) {
    display.drawLine(display.width()-1, display.height()-1, 0, i, SSD1306_WHITE);
    display.display();
    delay(1);
  }
  delay(250);

  display.clearDisplay();

  for(i=0; i<display.height(); i+=4) {
    display.drawLine(display.width()-1, 0, 0, i, SSD1306_WHITE);
    display.display();
    delay(1);
  }
  for(i=0; i<display.width(); i+=4) {
    display.drawLine(display.width()-1, 0, i, display.height()-1, SSD1306_WHITE);
    display.display();
    delay(1);
  }

  delay(2000); // Pause for 2 seconds
}

void testdrawrect(void) {
  display.clearDisplay();

  for(int16_t i=0; i<display.height()/2; i+=2) {
    display.drawRect(i, i, display.width()-2*i, display.height()-2*i, SSD1306_WHITE);
    display.display(); // Update screen with each newly-drawn rectangle
    delay(1);
  }

  delay(2000);
}

void testfillrect(void) {
  display.clearDisplay();

  for(int16_t i=0; i<display.height()/2; i+=3) {
    // The INVERSE color is used so rectangles alternate white/black
    display.fillRect(i, i, display.width()-i*2, display.height()-i*2, SSD1306_INVERSE);
    display.display(); // Update screen with each newly-drawn rectangle
    delay(1);
  }

  delay(2000);
}

void testdrawcircle(void) {
  display.clearDisplay();

  for(int16_t i=0; i<max(display.width(),display.height())/2; i+=2) {
    display.drawCircle(display.width()/2, display.height()/2, i, SSD1306_WHITE);
    display.display();
    delay(1);
  }

  delay(2000);
}

void testfillcircle(void) {
  display.clearDisplay();

  for(int16_t i=max(display.width(),display.height())/2; i>0; i-=3) {
    // The INVERSE color is used so circles alternate white/black
    display.fillCircle(display.width() / 2, display.height() / 2, i, SSD1306_INVERSE);
    display.display(); // Update screen with each newly-drawn circle
    delay(1);
  }

  delay(2000);
}

void testdrawroundrect(void) {
  display.clearDisplay();

  for(int16_t i=0; i<display.height()/2-2; i+=2) {
    display.drawRoundRect(i, i, display.width()-2*i, display.height()-2*i,
      display.height()/4, SSD1306_WHITE);
    display.display();
    delay(1);
  }

  delay(2000);
}

void testfillroundrect(void) {
  display.clearDisplay();

  for(int16_t i=0; i<display.height()/2-2; i+=2) {
    // The INVERSE color is used so round-rects alternate white/black
    display.fillRoundRect(i, i, display.width()-2*i, display.height()-2*i,
      display.height()/4, SSD1306_INVERSE);
    display.display();
    delay(1);
  }

  delay(2000);
}

void testdrawtriangle(void) {
  display.clearDisplay();

  for(int16_t i=0; i<max(display.width(),display.height())/2; i+=5) {
    display.drawTriangle(
      display.width()/2  , display.height()/2-i,
      display.width()/2-i, display.height()/2+i,
      display.width()/2+i, display.height()/2+i, SSD1306_WHITE);
    display.display();
    delay(1);
  }

  delay(2000);
}

void testfilltriangle(void) {
  display.clearDisplay();

  for(int16_t i=max(display.width(),display.height())/2; i>0; i-=5) {
    // The INVERSE color is used so triangles alternate white/black
    display.fillTriangle(
      display.width()/2  , display.height()/2-i,
      display.width()/2-i, display.height()/2+i,
      display.width()/2+i, display.height()/2+i, SSD1306_INVERSE);
    display.display();
    delay(1);
  }

  delay(2000);
}

void testdrawchar(void) {
  display.clearDisplay();

  display.setTextSize(1);      // Normal 1:1 pixel scale
  display.setTextColor(SSD1306_WHITE); // Draw white text
  display.setCursor(0, 0);     // Start at top-left corner
  display.cp437(true);         // Use full 256 char 'Code Page 437' font

  // Not all the characters will fit on the display. This is normal.
  // Library will draw what it can and the rest will be clipped.
  for(int16_t i=0; i<256; i++) {
    if(i == '\n') display.write(' ');
    else          display.write(i);
  }

  display.display();
  delay(2000);
}

void testdrawstyles(void) {
  display.clearDisplay();

  display.setTextSize(1);             // Normal 1:1 pixel scale
  display.setTextColor(SSD1306_WHITE);        // Draw white text
  display.setCursor(0,0);             // Start at top-left corner
  display.println(F("Hello, world!"));

  display.setTextColor(SSD1306_BLACK, SSD1306_WHITE); // Draw 'inverse' text
  display.println(3.141592);

  display.setTextSize(2);             // Draw 2X-scale text
  display.setTextColor(SSD1306_WHITE);
  display.print(F("0x")); display.println(0xDEADBEEF, HEX);

  display.display();
  delay(2000);
}

void testscrolltext(void) {
  display.clearDisplay();

  
  display.setTextSize(2); // Draw 2X-scale text
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(10, 0);
  display.println(F("scroll"));
  display.display();      // Show initial text
  delay(100);

  // Scroll in various directions, pausing in-between:
  display.startscrollright(0x00, 0x0F);
  delay(2000);
  display.stopscroll();
  delay(1000);
  display.startscrollleft(0x00, 0x0F);
  delay(2000);
  display.stopscroll();
  delay(1000);
  display.startscrolldiagright(0x00, 0x07);
  delay(2000);
  display.startscrolldiagleft(0x00, 0x07);
  delay(2000);
  display.stopscroll();
  delay(1000);
}

void testdrawbitmap(void) {
  display.clearDisplay();

  display.drawBitmap(
    (display.width()  - LOGO_WIDTH ) / 2,
    (display.height() - LOGO_HEIGHT) / 2,
    logo_bmp, LOGO_WIDTH, LOGO_HEIGHT, 1);
  display.display();
  delay(1000);
}

#define XPOS   0 // Indexes into the 'icons' array in function below
#define YPOS   1
#define DELTAY 2

void testanimate(const uint8_t *bitmap, uint8_t w, uint8_t h) {
  int8_t f, icons[NUMFLAKES][3];

  // Initialize 'snowflake' positions
  for(f=0; f< NUMFLAKES; f++) {
    icons[f][XPOS]   = random(1 - LOGO_WIDTH, display.width());
    icons[f][YPOS]   = -LOGO_HEIGHT;
    icons[f][DELTAY] = random(1, 6);
    Serial.print(F("x: "));
    Serial.print(icons[f][XPOS], DEC);
    Serial.print(F(" y: "));
    Serial.print(icons[f][YPOS], DEC);
    Serial.print(F(" dy: "));
    Serial.println(icons[f][DELTAY], DEC);
  }

  for(;;) { // Loop forever...
    display.clearDisplay(); // Clear the display buffer

    // Draw each snowflake:
    for(f=0; f< NUMFLAKES; f++) {
      display.drawBitmap(icons[f][XPOS], icons[f][YPOS], bitmap, w, h, SSD1306_WHITE);
    }

    display.display(); // Show the display buffer on the screen
    delay(200);        // Pause for 1/10 second

    // Then update coordinates of each flake...
    for(f=0; f< NUMFLAKES; f++) {
      icons[f][YPOS] += icons[f][DELTAY];
      // If snowflake is off the bottom of the screen...
      if (icons[f][YPOS] >= display.height()) {
        // Reinitialize to a random position, just off the top
        icons[f][XPOS]   = random(1 - LOGO_WIDTH, display.width());
        icons[f][YPOS]   = -LOGO_HEIGHT;
        icons[f][DELTAY] = random(1, 6);
      }
    }
  }
}
*/