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

#define Pin_JS_SEL 34
#define Pin_JS_HORZ 36
#define Pin_JS_VERT 39
#define Pin_ROTARY_CLK 35
#define Pin_ROTARY_DT 32
#define Pin_ROTARY_SW 33
#define Pin_LDR_DO 12
#define Pin_LDR_AO 13
#define Pin_USONIC_ECHO 27
#define Pin_USONIC_TRIG 14
#define Pin_SERVO_PWM 25
#define Pin_ANALOG_1 15
#define Pin_ANALOG_2 2
#define Pin_ANALOG_3 4

Adafruit_SSD1306 OLED(128, 64, &Wire, -1);

String Text[] = {"", "", ""};
// Default values (Servo, Ultrasonic, Loadcell, Temperature, Gyro, ...)
int Set[4][16] = {
{0, 500, 343, 0, 3026, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
{0, 1900, 2, 5, 330, 120, 120, 120, 9807, 9807, 9807, 0, 255, 0, 0, 0},
{0, 4500, 10, 2100, 788, 7860, 7860, 7860, 4096, 4096, 4096, 4096, 4095, 0, 0, 0},
{0, 4095, 0, 0, 6120, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}};
int Mode[8] = {0, 0, 0, 0, 0, 0, 0, 0};
int Data[5][16] = {
{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}};
float SData[5][16] = {
{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}};
float TData[6] = {0, 0, 0, 0, 0, 0};

void setup() {
pinMode(Pin_JS_SEL, INPUT);
pinMode(Pin_JS_HORZ, INPUT);
pinMode(Pin_JS_VERT, INPUT);
pinMode(Pin_ROTARY_CLK, INPUT);
pinMode(Pin_ROTARY_DT, INPUT);
pinMode(Pin_ROTARY_SW, INPUT_PULLUP);
Serial.begin(9600);
OLED_start();
I2C_start();
Mode[0] = 21;
}

void loop() {
if (digitalRead(Pin_ROTARY_SW) == LOW) {ROTARY_read_data();}
switch (Mode[0]) {
case 11: MPU6050_reset(); break;
case 12: GY271_reset(); break;
case 21: JS_read_motion(); break;
case 22: USONIC_read_data(); break;
case 31: MPU6050_read_data(); MPU6050_print_data(); break;
case 32: GY271_read_data(); GY271_print_data(); break;
}
//Serial.println(Text[0]);
//Serial.println(Text[1]);
}

void OLED_start() {
OLED.begin(SSD1306_SWITCHCAPVCC, 0x3C);
OLED_print_text(3, "Welcome!! I2C", 0, 2, 0, 0);
delay(2000);
OLED_print_text(6, "", 0, 0, 0, 0);
}

void OLED_print_text(int type, String text, int value, int size, int col, int row) {
OLED.setTextColor(WHITE);
OLED.setTextSize(size);
OLED.setCursor(col, row);
switch (type) {
case 1: OLED.println(value, DEC); break;
case 2: OLED.println(text); break;
case 3: OLED.clearDisplay(); OLED.println(text); OLED.display(); break;
case 4: OLED.clearDisplay(); OLED.println(value, DEC); OLED.display(); break;
case 5: OLED.println(value, HEX); break;
case 6: OLED.clearDisplay(); OLED.display(); break;
}}

void I2C_start() {
Wire.begin();
OLED_print_text(3, "I2C scan", 0, 2, 0, 0);
delay(2000);
for (byte j = 8; j < 120; j++) {
Wire.beginTransmission (j);
if (Wire.endTransmission () == 0) {
OLED.clearDisplay();
OLED_print_text(2, "Addr:", 0, 2, 0, 20);
OLED_print_text(1, "", j, 2, 70, 20);
OLED_print_text(2, " (0x", 0, 2, 0, 40);
OLED_print_text(5, "", j, 2, 50, 40);
OLED_print_text(2, ")", 0, 2, 80, 40);
OLED.display();
delay(2000);
}}
OLED_print_text(3, "", 0, 2, 90, 0);
}

void MPU6050_reset() {
OLED_print_text(3, "MPU6050 reset ..", 0, 2, 0, 0);
Wire.beginTransmission(0x68);
Wire.write(0x6B);
Wire.write(0x00);
Wire.endTransmission();
Wire.beginTransmission(0x68);
Wire.write(0x1C);
Wire.write(0x10);
Wire.endTransmission();
Wire.beginTransmission(0x68);
Wire.write(0x1B);
Wire.write(0x08);
Wire.endTransmission();
Wire.beginTransmission(0x68);
Wire.write(0x3B);
Wire.endTransmission();
Wire.requestFrom(0x68,14);
while(Wire.available() < 14);
Set[0][8] = Wire.read()<<8|Wire.read();
Set[0][9] = Wire.read()<<8|Wire.read();
Set[0][10] = Wire.read()<<8|Wire.read();
Set[0][11] = Wire.read()<<8|Wire.read();
Set[0][5] = Wire.read()<<8|Wire.read();
Set[0][6] = Wire.read()<<8|Wire.read();
Set[0][7] = Wire.read()<<8|Wire.read();
delay(3000);
OLED_print_text(3, "Reset completed.", 0, 2, 0, 0);
delay(1000);
}

void MPU6050_read_data() {
Wire.beginTransmission(0x68);
Wire.write(0x3B);
Wire.endTransmission();
Wire.requestFrom(0x68,14);
while(Wire.available() < 14);
SData[0][8] = Wire.read()<<8|Wire.read();
SData[0][9] = Wire.read()<<8|Wire.read();
SData[0][10] = Wire.read()<<8|Wire.read();
SData[0][11] = Wire.read()<<8|Wire.read();
SData[0][5] = Wire.read()<<8|Wire.read();
SData[0][6] = Wire.read()<<8|Wire.read();
SData[0][7] = Wire.read()<<8|Wire.read();
SData[1][5] = (1 + SData[0][5] - Set[0][5]) * Set[1][5] / Set[2][5];
SData[1][6] = (1 + SData[0][6] - Set[0][6]) * Set[1][6] / Set[2][6];
SData[1][7] = (1 + SData[0][7] - Set[0][7]) * Set[1][7] / Set[2][7];
SData[1][8] = (1 + SData[0][8] - Set[0][8]) * Set[1][8] / Set[2][8] / 1000;
SData[1][9] = 9.81 + (1 + SData[0][9] - Set[0][9]) * Set[1][9] / Set[2][9] / 1000;
SData[1][10] = (1 + SData[0][10] - Set[0][10]) * Set[1][10] / Set[2][10] / 1000;
SData[1][11] = (1 + SData[0][11] - Set[0][11]) * Set[1][11] / Set[2][11];
}

void MPU6050_print_data() {
TData[0] = SData[1][5];
TData[1] = SData[1][6];
TData[2] = SData[1][7];
TData[3] = SData[1][8];
TData[4] = SData[1][9];
TData[5] = SData[1][10];
}

void GY271_reset() {
OLED_print_text(3, "GY271 reset ....", 0, 2, 0, 0);
Wire.beginTransmission(0x1E);
Wire.write(0x02);
Wire.write(0x00);
Wire.endTransmission();
Wire.beginTransmission(0x1E);
Wire.write(0x03);
Wire.endTransmission();
Wire.requestFrom(0x1E,6);
while(Wire.available() < 6);
Set[0][13] = Wire.read()<<8|Wire.read();
Set[0][14] = Wire.read()<<8|Wire.read();
Set[0][15] = Wire.read()<<8|Wire.read();
delay(2000);
OLED_print_text(3, "Reset completed.", 0, 2, 0, 0);
delay(1000);
}

void GY271_read_data() {
Wire.beginTransmission(0x1E);
Wire.write(0x03);
Wire.endTransmission();
Wire.requestFrom(0x1E,6);
while(Wire.available() < 6);
SData[1][13] = Wire.read()<<8|Wire.read();
SData[1][14] = Wire.read()<<8|Wire.read();
SData[1][15] = Wire.read()<<8|Wire.read();
}

void GY271_print_data() {
TData[0] = SData[1][13];
TData[1] = SData[1][14];
TData[2] = SData[1][15];
}

void GY68_reset() {
OLED_print_text(3, "GY68 reset ....", 0, 2, 0, 0);
Wire.beginTransmission(0x77);
Wire.write(0x04);
Wire.write(0x00);
Wire.endTransmission();
Wire.beginTransmission(0x77);
Wire.write(0x05);
Wire.endTransmission();
Wire.requestFrom(0x77,6);
while(Wire.available() < 6);
Set[0][13] = Wire.read()<<8|Wire.read();
Set[0][14] = Wire.read()<<8|Wire.read();
Set[0][15] = Wire.read()<<8|Wire.read();
delay(2000);
OLED_print_text(3, "Reset completed.", 0, 2, 0, 0);
delay(1000);
}

void GY68_read_data() {
Wire.beginTransmission(0x77);
Wire.write(0x05);
Wire.endTransmission();
Wire.requestFrom(0x77,6);
while(Wire.available() < 6);
SData[1][13] = Wire.read()<<8|Wire.read();
SData[1][14] = Wire.read()<<8|Wire.read();
SData[1][15] = Wire.read()<<8|Wire.read();
}

void GY68_print_data() {
TData[0] = SData[1][13];
TData[1] = SData[1][14];
TData[2] = SData[1][15];
}

void JS_read_motion() {
int x = analogRead(Pin_JS_HORZ);
int y = analogRead(Pin_JS_VERT);
int r = analogRead(Pin_JS_SEL);
if (x > 3000) {Data[1][1] += 1;}
if (x < 1000) {Data[1][1] -= 1;}
if (y > 3000) {Data[2][1] += 1;}
if (y < 1000) {Data[2][1] -= 1;}
if (r < 100) {Data[3][1] += 1;}
OLED.clearDisplay();
OLED_print_text(1, "", Data[1][1], 3, 0, 0);
OLED_print_text(1, "", Data[2][1], 3, 50, 0);
OLED_print_text(1, "", Data[3][1], 3, 100, 0);
OLED.display();
}

void ROTARY_read_data() {
OLED.clearDisplay();
OLED_print_text(2, "OKKK", 0, 3, 0, 0);
OLED.display();
delay(3000);
}

void USONIC_read_data() {

}