#include <U8g2lib.h>
#include <Wire.h>
#include "gear.h"

U8G2_SSD1306_128X64_NONAME_F_HW_I2C u8g2(U8G2_R0, U8X8_PIN_NONE);


int left_top = A0;
int left_bottom = A1;
int right_top = A2;
int right_bottom = A3;

void setup()
{
  pinMode(left_top, INPUT);
  pinMode(left_bottom, INPUT);
  pinMode(right_top, INPUT);
  pinMode(right_bottom, INPUT);
  Serial.begin(9600);
  u8g2.begin();
}

void loop()
{
  u8g2.clearBuffer();

  u8g2.setFont(u8g2_font_ncenB14_tr);
  u8g2.setCursor(0,15);

  if(analogRead(left_top)>600 && analogRead(right_top)>600)
  {
    Serial.println("gear 3");
    u8g2.drawXBMP( 0, 0, 128, 64, epd_bitmap_gear_3);
  }

  else if(analogRead(left_bottom)>600 && analogRead(right_bottom)>600)
  {
    Serial.println("gear 4");
    u8g2.drawXBMP( 0, 0, 128, 64, epd_bitmap_gear_4);
  }

  else if(analogRead(left_top)>600)
  {
    Serial.println("gear 1");
    u8g2.drawXBMP( 0, 0, 128, 64, epd_bitmap_gear_1);
  }
  else if(analogRead(left_bottom)>600)
  {
    Serial.println("gear 2");
    u8g2.drawXBMP( 0, 0, 128, 64, epd_bitmap_gear_2);
  }

  else if(analogRead(right_top)>600)
  {
    Serial.println("gear 5");
    u8g2.drawXBMP( 0, 0, 128, 64, epd_bitmap_gear_5);
  }
  else if(analogRead(right_bottom)>600)
  {
    Serial.println("gear R");
    u8g2.drawXBMP( 0, 0, 128, 64, epd_bitmap_reverse);
  }
  else{
    Serial.println("Neutral");
    u8g2.drawXBMP( 0, 0, 128, 64, epd_bitmap_neutral);
  }

  u8g2.sendBuffer();
}