#include <Wire.h>
#include <Adafruit_SSD1306.h>
#include <Adafruit_GFX.h>
#include <U8g2_for_Adafruit_GFX.h>

Adafruit_SSD1306 display(128, 64, &Wire, 4);
U8G2_FOR_ADAFRUIT_GFX u8g2_for_adafruit_gfx;

void setup() {
  Serial.begin(9600);
  if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    Serial.println(F("SSD1306 allocation failed"));
    for (;;);
  }
  u8g2_for_adafruit_gfx.begin(display);
  display.clearDisplay();
}
const uint8_t testcapex[2015] U8G2_FONT_SECTION("testcapex") = 
  ";\0\4\5\5\5\5\6\7$\35\0\0\32\376\35\3\3\253\0\0\7\302 \6\0@\260\11!\24\211"
  "B\63I\245\61''\61\344\354\201>\304\30\263\1\0\42\26\16I\31\32\305\20RL\21F\264\302\30"
  "\202\206\31j\230\201\10#\61\225B\263Z\307\30u\310Q\306\34t\314A\307\30\63\311l\16\71\352\220"
  "\243\214\71\350\30\203\246\230d\232c\214:\344\250C\16:\346\240C\216\2\0$)U\303\261\272\306\3"
  "P<\20\305>\5\215\234\30\17\300\341\207C\25+\310\15\77\311$\322H\3\325\263\306\3P<\20\305"
  "\2%=\232j\323\14\207!\205\224q\10\35i\314\241\206\34y\310\241G\34\353p!F\23r<\60"
  "G,vDCG<rHAF\34b\220\21\306\24U\204AO\30\3EQ\216\20\267\20\0&*"
  "\326B\362j\316A\64\315\64\322\30v\314Q\307\34e\24t\15>\6\225a\306\34u\314Q\307TA"
  "\11m$\67\36\200\243\0'\12\6I\31\31\245\210f\14(\32J\303Qy\203\21U\220i\316\306\230"
  "\323\30s\66\306\234\215AfDE\2)\36Q\307Qz\11*\247XP\305\232\323\223\32k\350I\15"
  "=SC\317\324\250\305T\7\0*\31SE\230\232\205\23\17\0\201QH\304\230!\306\30e\304Q\16"
  "\35\1\0+\24N\301\325Y\6\32XT\23\214\60\321T\201\6\26\6\0,\20\6\301\61\11\204\24Q"
  "\304\62\203\20\62\10\0-\12\215@\327\31\254\300\2\0.\12\246@\63\11W\24\1\0/:\231Fs"
  "\373\200 \17\304\361\200\34\17\214\361\300\34\17\310\361\200\34\17\310\361\200\34\17\310\361\300\30\17\214\361\300"
  "\34\17\310\361\200\34\17\310\361\200\34\17\214\361\300\30\17\10\362\200\0\60,\225B\263j\216A#'\206"
  "\35r\20\42\207(b\310\22G\20a\310\22\207(rH\62\206$s\310Q\207L\42\215D\217\71\6"
  "\0\61\27\212J\223\31\210$\321QA\307\34c\314\331\30s\66\306\234\15\0\62\33\225B\263JPE"
  "#\367\0\30~\42\210\42\211\10*\303O>\211\64r\3\0\63\35\225B\263J\20I#\347'\67r"
  "\321\204\223]\36\10\303O\62\211\64\322@\365\30\0\64$\224F\263:G\25r\224!G\31b\230\21"
  "\207\31a\330\21G\35a\230&\322D\3\365Y\33}\306\7\1\65\34\225B\263J\261\221\23\343\1\70"
  "\374p\250b\5\271\341'\231D\32i\240z\14\0\66#\225B\263jOA#'\306\3p\370\341\20"
  "I\63\311$\206\31r\324!G\35\62\211\64\22=\346\30\0\67\32\222J\223\32\261\370\300b\323\36\267"
  "\330\352\222,\330\340\242Klp\321\205\2\70%\225B\263j\216A#'\206\35r\324!G\31\4Y"
  "sOAd\230!G\35r\324!\223H#\15d\216\1\71!\225B\263j\216A#'\306\31r\324"
  "!G\35\42\215DQAn\370I&\221F\32\250\36\3\0:\22\351A\63)gb\310\361`q\210"
  "\61\246)\10\0;\26I\302\61)gb\310\361`ATA\306\30S\330\250\10\2\0<\26\217A\365"
  "\351A\32\245H\23\215 \227X\23\315!Kt\60\0=\20.\301\325)l\301<\20L\64\301\10#"
  "\0>\26\217A\365\71A\33\251T\63\314\35\245H\23\314 Gl\340\0\77\31\222J\223\32\261\370\300"
  "b\323\36\267\330\312\24<\36,\216=\326\330S\2@\62\232bS\14Y|\360\1\364\0!\343\4\42"
  "O \361\210\61E\25q\210QE\34RX\21\207\334\250\262#\226\7\305\362\0(\17\204\362@(\17"
  "\4\0A)\225B\263j\216A#'\206\35r\324!G\35\42\311\234\30f\310Q\207\34u\310Q\207"
  "\30f\214Q\307\30uLa\307\0B%\225B\263JPE#\67\305\31r\324!G\35\62\305$\21"
  "Ae\330\21G\35r\324!\223H#\15T\217\1C\33\225B\263jOA#'\306\3p\370\341\206"
  "\237\334\360\63\237D\32\31A\4\0D*\225B\263JPE#\67\305\31r\324!G\35b\330\21\207"
  "\31r\324!G\35b\230!G\35r\324!\223H#\15T\217\1E\32\225B\263J\261\221\233\342\1"
  "\70\374\360\247\36{\231\341'\237D\32\271\1\0F\27\224B\223J\260\211\233\302\217>\372\241\247^e"
  "\364Y\33n\346\0G(\225B\263jOA#'\306\3p\370\341\206(r\210\42\207(bL\42\207"
  "\4r\252C\216:D\32i\42\202\314\61\0H\63\226B\323JG\31d\230\61G\35s\324\61G\35"
  "S\234\61G\35s\324\61\223L\63\33\303\16\71\314\230\243\216\71\352\30\303\14\62\352 \243\16\62\352 "
  "\0I\26\211B\63I\245\61\233b\16\71\305!'\61\344L\214\61\33\0J\31\221B\63\312\245\65\333"
  "b\17=\324\320\63\65\364$\217\70\343\14S\213\1K\64\225B\263JG\25dX\61G\31s\224\61"
  "G\31S\234!G\35r\324!SL\22\21T\206\21t\224\61G\35r\324!\206\31c\324\61F\35"
  "c\324\61\0L\30\223B\223J\7\33|\306E\37|\260\301gl\360\211\77\60B.\0MA\231B"
  "\63K\212\25\244X\61K\31\263\224\61K\31S\10A\207\34Q\314!G\24s\310\21\305\30rH!"
  "\247!\344\64\204\34bP\21\206\34\304\310A\214\34\304\210a\312\30\246\214a\312\30\246\14\0NP\237"
  "B\363K\12&\244\234\62\313)\263\330\62\213-S\10Q\214\34A\24#G\24S\304!G\24b\204"
  "!\207\224\342\220C\212\60\342\220C\212(\344\20c\224\61\344\30EN\223\314!\307$s\210Q\7\31"
  "c\324\61\307\30e\320\61F\25u\14\0O*\225B\263j\216A#'\206\35r\324!G\35b\230"
  "!G\35r\324!\206\35q\230!G\35r\324!\322H\23\21d\216\1P#\225B\263JPE#"
  "\67\305\31r\324!G\35\62\305$\21Ae\370\231\33\17\200\361\0\30\17\200\361\0Q\61\365\302\261j"
  "\216A#'\206\35r\324!G\35b\230!G\35r\324!\206\35q\230!G\35r\324!\322H\23"
  "\21d\316\26\17D\361@\24\13\0R,\225B\263JPE#\67\305\31r\324!G\35\62\305$\21"
  "Ae\30AG\31s\324!G\35b\230\61F\35c\324\61F\35\3\0S\37\225B\263j\17E#"
  "\223I\214\7\340p\303\243\212\12\252\250\17\77\211\64\322D\4\325c\0T\30\222Fs*P\304\205T"
  "\6\37{\254\261gk\354\231\36{Z#\1U\63\225B\263JG\25sX\61G\31s\224\61\206\31"
  "b\234!G\35r\324!\206\31r\324!G\35r\324!\206\31r\324!G\35\62\211\64\22=\346\30"
  "\0V\63\225B\263JG\25s\224\61G\31s\224!\207\31b\330!G\35b\330\21\207\31r\324!"
  "G\35b\230!G\35r\324!G\35\42\321\203\313&m(\0W\61\231b\63\14C\235\25RG!"
  "u\62\223\35\205\324\311\216:\314dG\35v\324a\206\35u\230aG\35v\324!\326Pe\4c\305"
  "$\70\230\241\0X\64\225B\263JG\25s\224\61G\31s\224\61\206\31b\330!G\35r\224A\220"
  "\65\347\30D\206\31r\324!G\35b\230\61F\35c\324\61F\35S\230A\0Y#\223F\223*G"
  "\25bX\21G\31q\224\21G\31Q\134SN}\320\310SM\36l\360\31\33|J\0Z\33\225B"
  "\263:\362dj\204\221E\61\262\310&\213l\302\207K#\233\210\42\2\0\0\0\0\4\377\377\0";

const uint8_t testcapext[200] U8G2_FONT_SECTION("testcapext") = 
  "\12\0\3\5\4\5\1\2\5\10\24\0\376\22\376\22\376\0\0\0\0\0\253\60\16H\203\241q\260 \361"
  "\377\177\36\250\1\61\15H\203\221\221\240!\363O\204\5g\62\22H\203\241q\260 \361d\250\30\331\12"
  "\231,\70\3\63\22H\203\241q\260 \361d\210X\261C\342\347\201\32\64\26H\203\301\215 )\201\6"
  "\5\32\24h\210 \361,\330\220I\31\65\22H\203\221Y$\223\202\20\26$\310L\342y\240\6\66\21"
  "H\203\241q\260 \361\311\350`A\342\317\3\65\67\21H\203\221Y\260\241Bf*V\310L\205\14q"
  "\70\20H\203\241q\260 \361W\202\203\304\237\7j\71\20H\203\241q\260 \361g\301\206\304\317\3\65"
  "\0\0\0\4\377\377\0";
int16_t  x, y;
uint16_t w, h;
void displayHUD() {
  //Benzina
  int serbatoio = analogRead(A1);
  serbatoio = map(serbatoio, 0, 1024, 0 ,123);
/*
  display.fillRect(1, 1, 126, 12, WHITE);
  display.fillRect(2, 2, 124, 10, BLACK);
  display.fillRect(3, 3, serbatoio, 8, WHITE);
  int space = 20;
  for(int i=0; i<6; i++){
    display.drawLine(space, 2, space, 10, BLACK);
    space = space+17;
  }
  */
  display.fillRect(0, 0, 128, 32, WHITE);
  display.fillRect(1, 1, 126, 30, BLACK);

    //Test speed

  int speed = analogRead(A0);
  speed = map(speed, 0, 1023, 0, 120);
  int angle = analogRead(A0);
  angle = map(speed, 0, 120, 0, 180);
    speed = speed*1.1;
  int cursorSpeed = 7;

  /*if(speed<10){
    cursorSpeed = 35;
  }else if(speed<20){
    cursorSpeed = 25;
  }else if(speed<100){
    cursorSpeed = 17;
  }else if(speed<100){
    cursorSpeed = 12;
  }else{
    cursorSpeed = 7;
  }*/

  String thisString = String(speed);
  String speedString = thisString;

  u8g2_for_adafruit_gfx.setFontMode(1);         
  u8g2_for_adafruit_gfx.setFontDirection(0);            
  u8g2_for_adafruit_gfx.setForegroundColor(WHITE);      
  u8g2_for_adafruit_gfx.setCursor(cursorSpeed, 32);
  u8g2_for_adafruit_gfx.setFont(testcapex);
  u8g2_for_adafruit_gfx.print(speedString);

  //Draw Speed
  /* display.drawCircle(83, 24, 21, WHITE);
  display.fillCircle(83, 24, 21, WHITE);
  display.drawCircle(83, 24, 18, BLACK);
  display.fillCircle(83, 24, 18, BLACK);
  float x1=sin((angle - 90)*M_PI/180.);
  float y1=cos((angle - 90)*M_PI/180.);
  display.fillRect(2, 29, 125, 3 , BLACK);
  display.drawLine(83, 28, 83+12*x1, 28-12*y1, WHITE);
  display.getTextBounds(thisString, 0, 0, &x, &y, &w, &h);
  display.setCursor((128-w)/2+1, 52); 
  display.fillRect(0, 32, 128, 64, BLACK);
  display.drawRect(0, 0, 128, 32, WHITE);

  u8g2_for_adafruit_gfx.setCursor(110, 20);
  u8g2_for_adafruit_gfx.setFont(testcapext);
  u8g2_for_adafruit_gfx.print(speedString); */
  //display.println(thisString);
}

void loop() {
  display.clearDisplay();
  display.setTextSize(2);
  display.setTextColor(WHITE);
  displayHUD();
  display.display();

}