#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};


float ang = 0;
float ang1 = 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 = -30;


void setup() {

 

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

  

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

  
}

void loop() {

  sensors_event_t a, g, temp;
  mpu.getEvent(&a, &g, &temp);
  
  float x = a.acceleration.x/9.80665;
  
  u8g.firstPage();

  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;

    float x_start1 = r_s * cos(radians(ang1)) + center_x;
    float y_start1 = r_s *  sin(radians(ang1)) + center_y;    
    float x_end1 = r_e * cos(radians(ang1)) + center_x;
    float y_end1 = r_e *  sin(radians(ang1)) + center_y;

    u8g.drawLine(x_start, y_start, x_end, y_end);
    u8g.drawLine(x_start1+3, y_start1, x_end1+3, y_end1);

    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);


int speed_pos_x =64 - (w/2);

u8g.setPrintPos(speed_pos_x+2, 37+11);


u8g.setPrintPos(0, 11+11);


float incli = a.acceleration.x;
float inclix;
u8g.setPrintPos(speed_pos_x+2, 62+11);
if (incli < 0){
  incli = incli * (-1);
} else {incli = incli;}

if (incli > 9.80665){
  inclix = incli - 9.80665;
u8g.print(degrees(asin(inclix/9.80665))+90);
}else{u8g.print(degrees(asin(incli/9.80665)));}




if (ang >= -30.00 & ang <= 30.00){
digitalWrite( relayPin, LOW);


} else{ u8g.setColorIndex(1);
u8g.setFont(u8g_font_8x13r);
u8g.drawStr(0,10,"VAI CAPOTAR");


digitalWrite( relayPin, HIGH);}

u8g.setColorIndex(3);

u8g.drawDisc(64,32,5);
 
  
    
    
  } while ( u8g.nextPage() );

  


incli1 = a.acceleration.x*57.296;


ang = asin(x)*(180/3.1416);
ang1 = ang + 90;
  
}