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

U8G2_SSD1306_128X64_NONAME_F_HW_I2C u8g2(U8G2_R0, U8X8_PIN_NONE); // initialization for the used OLED display

int current_gear = 0; //-1 = R, 0 = N, 1 = 1, 2 = 2, ...  
int switch_mode = 0;//mode 0 = debug screen, mode 1 = full screen

void setup() 
{
  //read analog voltage
  pinMode(A0, INPUT); 
  pinMode(A2, INPUT); 
  pinMode(A3, INPUT); 
  pinMode(A6, INPUT); 

  //Serial.begin(115200); //might not be needed? but why
  u8g2.begin(); //start u8g2 library
}

void loop() 
{
  switch_mode = digitalRead(2); //set screen layout based on digital 2 state

  //read analog val val will be 0-1023
  int hall_val_0 = analogRead(A0);
  int hall_val_1 = analogRead(A1);
  int hall_val_2 = analogRead(A2);
  int hall_val_3 = analogRead(A3);
  int hall_val_4 = analogRead(A6);

  //convert hall val to percentage 0-100%
  int percent_val_0 = round(abs(hall_val_0 - 512) / 5.12);
  int percent_val_1 = round(abs(hall_val_1 - 512) / 5.12);
  int percent_val_2 = round(abs(hall_val_2 - 512) / 5.12);
  int percent_val_3 = round(abs(hall_val_3 - 512) / 5.12);
  int percent_val_4 = round(abs(hall_val_4 - 512) / 5.12);

  u8g2.clearBuffer();	//clear the internal memory

  u8g2.setFont(u8g2_font_pressstart2p_8u);

  if (switch_mode == 0) //debug screen
  {
    u8g2.clearBuffer();

    u8g2.setCursor(0, 20);
    u8g2.print(percent_val_0); //print hall 0 val

    u8g2.setCursor(0, 20 + 32);
    u8g2.print(percent_val_1); //print hall 1 val

    u8g2.setCursor(0 + 32, 20);
    u8g2.print(percent_val_2);//print hall 2 val

    u8g2.setCursor(0 + 32, 20 + 32);
    u8g2.print(percent_val_3);//print hall 3 val

    u8g2.setCursor(0 + 64, 20);
    u8g2.print(percent_val_4);//print hall 4 val
  }

  else //h shift pattern
  {
    u8g2.clearBuffer();
    u8g2.drawXBMP(0, 0, 128, 64, bitmaps_h_pattern[current_gear+1]);
  }

  //if else statements for gear selection
  if (percent_val_0 > 30 && percent_val_2 > 30)  //third gear
    {
      current_gear = 3;
    }
  else if (percent_val_2 > 30 && percent_val_4 > 30) //fifth gear
    {
     current_gear = 5; 
    }
  else if (percent_val_1 > 30 && percent_val_3 > 30) //fourth gear
    {
     current_gear = 4; 
    }
  else if (percent_val_0 > 30) //first gear
    {
     current_gear = 1;
    }
  else if (percent_val_1 > 30) //second gear
    {
     current_gear = 2;
    }
  else if (percent_val_3 > 30) //sixth gear
    {
     current_gear = 6;
    }
  else if (percent_val_4 > 30) //reverse gear
    {
     current_gear = -1;
    }
  else //Neutral
    {
      current_gear = 0;
    }

  u8g2.sendBuffer(); //transfer internal memory to the display
}