#include "U8glib.h"
#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <Wire.h>

U8GLIB_SSD1306_128X64 u8g(U8G_I2C_OPT_DEV_0 | U8G_I2C_OPT_NO_ACK | U8G_I2C_OPT_FAST); // Fast I2C / TWI

Adafruit_MPU6050 mpu;


const u8g_fntpgm_uint8_t tah[238] U8G_FONT_SECTION("tah") = {
  0,11,12,0,11,13,0,0,0,0,45,57,0,24,0,13,
  0,3,1,1,4,0,16,224,1,1,1,2,0,11,128,3,
  11,11,4,0,11,32,32,32,64,64,64,64,64,128,128,128,
  7,13,13,9,0,11,56,68,130,130,130,130,130,130,130,130,
  130,68,56,5,13,13,9,1,11,224,32,32,32,32,32,32,
  32,32,32,32,32,248,7,13,13,9,0,11,56,68,130,130,
  130,2,4,8,16,32,64,128,254,7,13,13,9,0,11,56,
  68,130,130,2,4,56,4,2,130,130,68,56,7,13,13,9,
  0,11,8,8,16,16,32,34,66,66,130,254,2,2,2,7,
  13,13,9,0,11,252,128,128,128,128,248,4,2,130,130,130,
  68,56,7,13,13,9,0,11,56,68,130,128,128,184,196,130,
  130,130,130,68,56,6,13,13,9,1,11,252,4,8,8,8,
  16,16,32,32,64,64,64,128,7,13,13,9,0,11,56,68,
  130,130,130,68,56,68,130,130,130,68,56,7,13,13,9,0,
  11,56,68,130,130,130,130,70,58,2,2,130,68,56};

  const u8g_fntpgm_uint8_t digit_small[145] U8G_FONT_SECTION("digit_small") = {
  0,4,4,0,4,5,0,0,0,0,45,57,0,9,0,5,
  0,0,0,0,4,0,0,0,0,0,4,0,0,0,0,0,
  4,0,0,3,5,5,4,0,4,96,160,160,160,192,3,5,
  5,4,0,4,64,192,64,64,224,3,5,5,4,0,4,192,
  32,64,128,224,3,5,5,4,0,4,224,32,64,32,192,3,
  5,5,4,0,4,128,160,224,32,32,3,5,5,4,0,4,
  224,128,224,32,192,3,5,5,4,0,4,64,128,192,160,224,
  3,5,5,4,0,4,224,32,32,64,64,3,5,5,4,0,
  4,96,160,64,160,192,3,5,5,4,0,4,224,160,96,32,
  64};

int sensor_value = 0;
float ang = 0;
const int relayPin = 7;
float incli1 =0;

int center_x = 64;     // x center of the knob 
int center_y = 32;
int r_e = 30;
int r_s = 20;


void setup() {

  pinMode( relayPin, OUTPUT);
  mpu.begin(0x68);

  u8g.setFont(u8g_font_tpssb);
  u8g.setColorIndex(1);

  //Serial.begin(9600);
}

void loop() {

  sensors_event_t a, g, temp;
  mpu.getEvent(&a, &g, &temp);

  u8g.firstPage();

  //Serial.println(analogRead(A0));

  

  do {
    
    float x_start = r_s * cos(radians(ang)) + center_x;
    float y_start = r_s *  sin(radians(ang)) + center_y;    
    float x_end = r_e * cos(radians(ang)) + center_x;
    float y_end = r_e *  sin(radians(ang)) + center_y;

    u8g.drawLine(x_start, y_start, x_end, y_end);

    int speed_mph = map(ang, 180, 360, 0 , 320);

u8g.setColorIndex(1);
u8g.setFont(tah);
speed_mph = (int)speed_mph;
String aStupidWasteOfResource = String(speed_mph);
char copy[5];
aStupidWasteOfResource.toCharArray(copy, 5);


int w = u8g.getStrWidth(copy);

// center = 27, 32
// calculate left side of speed number
int speed_pos_x =64 - (w/2);

u8g.setPrintPos(speed_pos_x+2, 37+11);
//u8g.drawPixel(16, 37);
u8g.print((int)speed_mph);

u8g.setPrintPos(0, 11+11);
u8g.print(sensor_value);

float incli = g.gyro.x*57.296;
u8g.setPrintPos(speed_pos_x+2, 64+11);
if (incli < 0){
u8g.print(incli*(-1));
} else {u8g.print(incli);}

if (sensor_value >= 288){


u8g.setColorIndex(1);
u8g.setFont(u8g_font_8x13r);
u8g.drawStr(95,10,"FUEL");
u8g.drawStr(95,20,"LOW!");

digitalWrite( relayPin, HIGH);
} else{ digitalWrite( relayPin, LOW);}

 
  
    
    
  } while ( u8g.nextPage() );

  //if (ang < 540) {
  //  ang = ang + 15;
  //  delay(50);
  //} else {
  //  ang = 180;
  //}

sensor_value = analogRead(A0);
incli1 = g.gyro.x*57.296;
ang = map(incli1, 0, -45, 180, 90);
  
}