#include <Wire.h>
#include "SPI.h"
#include <LiquidCrystal_I2C.h>
#include <Keypad.h>
#include "SD.h"
#include "HX711.h"

#define Pin_KNOB_SIG 15
#define Pin_SWITCH 13
#define Pin_ANALOG_Read 35
#define Pin_ANALOG_Write 12
#define Pin_DIGITAL_Read 34
#define Pin_DIGITAL_Write 26
#define Pin_PWM_Write 25
#define Pin_HX711_SCK 33
#define Pin_HX711_DT 32
uint8_t Pin_KEYPAD_Row[4] = {17, 16, 4, 2};
uint8_t Pin_KEYPAD_Col[4] = {3, 1, 27, 14};

const char Keys[4][4] =
{{ '1', '2', '3', 'A' },
{ '4', '5', '6', 'B' },
{ '7', '8', '9', 'C' },
{ '*', '0', '#', 'D' }};
// const int C[10] = {0, 36, 180, 3000, 1000, 8191, 4095, 2047, 1023, 255};
int Mode[8] = {0, 0, 0, 0, 0, 0, 0, 0};
// 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}};
float Data[4][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}};

LiquidCrystal_I2C LCD = LiquidCrystal_I2C(0x27, 16, 2);
Keypad KEYPAD(makeKeymap(Keys), Pin_KEYPAD_Row, Pin_KEYPAD_Col, 4, 4);
File file, root;
HX711 HX711;

void setup() {
pinMode(Pin_SWITCH, INPUT);
pinMode(Pin_DIGITAL_Read, INPUT);
pinMode(Pin_DIGITAL_Write, OUTPUT);
pinMode(Pin_PWM_Write, OUTPUT);
Serial.begin(9600);
Wire.begin();
start_LCD();
set_I2C_registry();
start_SDCARD();
HX711.begin(Pin_HX711_DT, Pin_HX711_SCK);
}

void loop() {
Mode[1] = digitalRead(Pin_SWITCH);
if (Mode[1] == HIGH) {get_KEYPAD_option(2);}
switch (Mode[0]) {
case 11: set_PWM_output(); break;
case 12: write_PWM_output(); break;
case 13: save_PWM_data(); break;
case 14: load_PWM_data(); break;
case 21: set_DIGITAL_IO(); break;
case 22: read_DIGITAL_IO(); break;
case 31: set_HX711_cal(); break;
case 32: read_HX711_input(); break;
case 41: set_ANALOG_cal(); break;
case 42: read_ANALOG_input(); break;
case 51: read_I2C_input(); break;
case 61: write_ANALOG_output(); break;
case 71: read_DIGITAL_input(); break;
case 81: write_DIGITAL_output(); break;
}}

void start_LCD() {
LCD.init();
LCD.backlight();
print_LCD_Char(1, 0, 0);
print_LCD_Char(2, 1, 0);
print_LCD_Char(3, 2, 0);
print_LCD_Char(4, 0, 1);
print_LCD_Char(5, 1, 1);
print_LCD_Char(6, 2, 1);
print_LCD_Char(7, 3, 0);
print_LCD_Char(8, 3, 1);
print_LCD_text(2, "Universal ", 0, 6, 0);
print_LCD_text(2, "Tester ...", 0, 6, 1);
for (int i = 0; i <= 6; i++) {
print_LCD_text(1, "", 6 - i, 15, 1);
delay(990);
}
print_LCD_text(4, "", 0, 0, 1);
}

void print_LCD_Char(int j, int col, int row) {
byte Char1[8] = {B11111, B10000, B10000, B10000, B10000, B10000, B10000, B10000};
byte Char2[8] = {B11111, B00000, B00100, B00100, B00100, B00100, B01110, B10101};
byte Char3[8] = {B11111, B00001, B00001, B00001, B00001, B00001, B00001, B00001};
byte Char4[8] = {B10001, B10010, B10100, B10000, B10000, B10000, B10000, B11111};
byte Char5[8] = {B00100, B00100, B00100, B00100, B00100, B01110, B00000, B11111};
byte Char6[8] = {B10001, B01001, B00101, B00001, B00001, B00001, B00001, B11111};
byte Char7[8] = {B10000, B11000, B11100, B11110, B11110, B11100, B11000, B10000};
byte Char8[8] = {B00001, B00011, B00111, B01111, B01111, B00111, B00011, B00001};
LCD.createChar(1, Char1);
LCD.createChar(2, Char2);
LCD.createChar(3, Char3);
LCD.createChar(4, Char4);
LCD.createChar(5, Char5);
LCD.createChar(6, Char6);
LCD.createChar(7, Char7);
LCD.createChar(8, Char8);
LCD.setCursor(col, row);
LCD.write(j);
}

void print_LCD_text(int type, char* text, float value, int col, int row) {
LCD.setCursor(col, row);
switch (type) {
case 1: LCD.println(value, DEC); break;
case 2: LCD.println(text); break;
case 3: LCD.clear(); LCD.println(text); break;
case 4: LCD.clear(); break;
}}

void set_I2C_registry() {
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();
}

void get_KEYPAD_input(int i) {
int j = 1;
String text = "";
while (text.length() < i) {
char keyin = KEYPAD.getKey();
if (keyin >= '0' && keyin <= '9') {
Data[2][j] = 0 - 48 + int(keyin);
print_LCD_text(1, "", Data[2][1], 12, 1);
print_LCD_text(1, "", Data[2][2], 13, 1);
print_LCD_text(1, "", Data[2][3], 14, 1);
print_LCD_text(1, "", Data[2][4], 15, 1);
text += keyin; j++;
}}
print_LCD_text(4, "", 0, 0, 0);
}

void get_KEYPAD_option(int i) {
int j = 0;
String text = "";
print_LCD_text(3, "Turn off switch", 0, 0, 0);
print_LCD_text(2, "Enter 10-99 > ", 0, 0, 1);
while (text.length() < i) {
char keyin = KEYPAD.getKey();
if (keyin >= '0' && keyin <= '9') {
Mode[j + 2] = 0 - 48 + int(keyin);
print_LCD_text(1, "", Mode[j + 1], 14, 1);
print_LCD_text(1, "", Mode[j + 2], 15, 1);
Mode[0] = 10*Mode[2] + Mode[3];
text += keyin; j++;}}
Mode[1] = 0;
print_LCD_text(4, "", 0, 0, 0);
}

void start_SDCARD() {
if (!SD.begin(5)) {Serial.println("SD card reader error"); return;}
uint8_t cardType = SD.cardType();
uint64_t cardSize = SD.cardSize() / (1024 * 1024);
if (cardType == CARD_NONE) {Serial.println("No SD card"); return;}
Serial.print("Storage : ");
if (cardType == CARD_MMC) {Serial.println("MMC");}
else if (cardType == CARD_SD) {Serial.println("SDSC");}
else if (cardType == CARD_SDHC) {Serial.println("SDHC");}
else {Serial.println("UNKNOWN");}
Serial.printf("Size : %lluMB\n", cardSize);
Serial.printf("Space : %lluMB\n", SD.totalBytes() / (1024 * 1024));
Serial.printf("Used : %lluMB\n", SD.usedBytes() / (1024 * 1024));
write_SDCARD_file("Add", "/Folder", "");
read_SDCARD_file("/", 0);
write_SDCARD_file("Remove", "/Folder", "");
read_SDCARD_file("/", 0);
save_SDCARD_data("Create", "/TestFile1.txt", "Test", 0);
write_SDCARD_file("Rename", "/TestFile1.txt", "/TestFile2.txt");
write_SDCARD_file("Delete", "/TestFile2.txt", "");
}

void read_SDCARD_file(const char* path, uint8_t levels) {
Serial.printf("Listing directory: %s\n", path);
root = SD.open(path);
if (!root) {Serial.println("ERROR: Open dir failed"); return;}
if (!root.isDirectory()) {Serial.println("Not a directory"); return;}
file = root.openNextFile();
while(file) {
if (file.isDirectory()) {Serial.println(file.name());
if (levels) {read_SDCARD_file(file.name(), levels -1);}}
else {
Serial.print(file.name());
Serial.print("\t");
Serial.print(file.size());
Serial.println(" kB");
}
file = root.openNextFile();
// file.read(static uint8_t Var1[512], size_t Var2);
// file.write(static uint8_t Var1[512], 512);
}}

void write_SDCARD_file(String type, char* path1, char* path2) {
int j = 0;
if (type == "Rename") {if (SD.rename(path1, path2)) {j = 1;}}
if (type == "Delete") {if (SD.remove(path1)) {j = 1;}}
if (type == "Remove") {if (SD.rmdir(path1)) {j = 1;}}
if (type == "Add") {if (SD.mkdir(path1)) {j = 1;}}
Serial.printf("%s ", type);
Serial.printf(": %s ", path1);
Serial.printf("> %s", path2);
if (j == 1) {Serial.println("... Done");}
else {Serial.println("... ERROR");}
}

void load_SDCARD_data(char* path) {
file = SD.open(path);
Serial.printf("Read : %s", path);
if (!file) {Serial.println("... No file"); return;}
Serial.println("... Complete");
while (file.available()) {Serial.write(file.read());}
file.close();
}

void save_SDCARD_data(String type, char* path, char* text, int data) {
if (type == "Create") {file = SD.open(path, FILE_WRITE);}
if (type == "Write") {file = SD.open(path, FILE_APPEND);}
Serial.printf("%s ", type);
Serial.printf(": %s ", path);
if (!file) {Serial.println("... No file"); return;}
if (file.print(text)) {Serial.println("... Saved");}
else if (file.print(data)) {file.print(",\n"); Serial.println("... Input");}
else {Serial.println("... ERROR");}
file.close();
}

void set_PWM_output() {
print_LCD_text(2, "Set const C1 :", 0, 0, 0);
print_LCD_text(2, "e.g.:0500>", 0, 0, 1);
get_KEYPAD_input(4);
Set[0][1] = 1000*Data[2][1] + 100*Data[2][2] + 10*Data[2][3] + Data[2][4];
print_LCD_text(2, "Set const C2 :", 0, 0, 0);
print_LCD_text(2, "e.g.:1900>", 0, 0, 1);
get_KEYPAD_input(4);
Set[1][1] = 1000*Data[2][1] + 100*Data[2][2] + 10*Data[2][3] + Data[2][4];
print_LCD_text(2, "Set const C3 :", 0, 0, 0);
print_LCD_text(2, "e.g.:4500>", 0, 0, 1);
get_KEYPAD_input(4);
Set[2][1] = 1000*Data[2][1] + 100*Data[2][2] + 10*Data[2][3] + Data[2][4];
Mode[0] = 12;
}

void write_PWM_output() {
Data[0][1] = analogRead(Pin_KNOB_SIG);
Data[1][1] = Set[0][1] + Data[0][1] * Set[1][1] / Set[3][1];
print_LCD_text(2, "Input  : ", 0, 0, 0);
print_LCD_text(1, "", Data[0][1], 10, 0);
print_LCD_text(2, "Output : ", 0, 0, 1);
print_LCD_text(1, "", Data[1][1], 10, 1);
for (int j = 0; j < 10; j++) {
digitalWrite(Pin_PWM_Write, HIGH);
delayMicroseconds(Data[1][1]);
digitalWrite(Pin_PWM_Write, LOW);
delayMicroseconds(Set[2][1]);
}}

void save_PWM_data() {
print_LCD_text(3, "Saving PWM data", 0, 0, 0);
save_SDCARD_data("Create", "/PWM.txt", "PWM Data Collection\n", 0);
save_SDCARD_data("Write", "/PWM.txt", "Data are as follow :\n", 0);
for (int j = 0; j < 16; j++) {
Data[0][1] = analogRead(Pin_KNOB_SIG);
Data[1][1] = Set[0][1] + Data[0][1] * Set[1][1] / Set[3][1];
Data[3][j] = Data[1][1];
save_SDCARD_data("Write", "/PWM.txt", "", Data[3][j]);
print_LCD_text(2, ">", 0, j, 1);
delay(990);
}
Mode[0] = 12;
}

void load_PWM_data() {
load_SDCARD_data("/PWM.txt");
Mode[0] = 12;
}

void set_DIGITAL_IO() {
print_LCD_text(2, "Set const C1 :", 0, 0, 0);
print_LCD_text(2, "e.g.:0343>", 0, 0, 1);
get_KEYPAD_input(4);
Set[0][2] = 1000*Data[2][1] + 100*Data[2][2] + 10*Data[2][3] + Data[2][4];
print_LCD_text(2, "Set const C2 :", 0, 0, 0);
print_LCD_text(2, "e.g.:0002>", 0, 0, 1);
get_KEYPAD_input(4);
Set[1][2] = 1000*Data[2][1] + 100*Data[2][2] + 10*Data[2][3] + Data[2][4];
print_LCD_text(2, "Set const C3 :", 0, 0, 0);
print_LCD_text(2, "e.g.:0010>", 0, 0, 1);
get_KEYPAD_input(4);
Set[2][2] = 1000*Data[2][1] + 100*Data[2][2] + 10*Data[2][3] + Data[2][4];
Mode[0] = 22;
}

void read_DIGITAL_IO() {
digitalWrite(Pin_DIGITAL_Write, LOW);
delayMicroseconds(Set[1][2]);
digitalWrite(Pin_DIGITAL_Write, HIGH);
delayMicroseconds(Set[2][2]);
digitalWrite(Pin_DIGITAL_Write, LOW);
Data[0][2] = pulseIn(Pin_DIGITAL_Read, HIGH);
Data[1][2] = (Data[0][2]/1000000) * Set[0][2] * 100 / 2;
print_LCD_text(2, "Raw data:  ", 0, 0, 0);
print_LCD_text(1, "", Data[0][2], 11, 0);
print_LCD_text(2, "Real data: ", 0, 0, 1);
print_LCD_text(1, "", Data[1][2], 11, 1);
}

void set_HX711_cal() {
print_LCD_text(2, "Set const C1 :", 0, 0, 0);
print_LCD_text(2, "e.g.:0000>", 0, 0, 1);
get_KEYPAD_input(4);
Set[0][3] = 1000*Data[2][1] + 100*Data[2][2] + 10*Data[2][3] + Data[2][4];
print_LCD_text(2, "Set const C2 :", 0, 0, 0);
print_LCD_text(2, "e.g.:0005>", 0, 0, 1);
get_KEYPAD_input(4);
Set[1][3] = 1000*Data[2][1] + 100*Data[2][2] + 10*Data[2][3] + Data[2][4];
print_LCD_text(2, "Set const C3 :", 0, 0, 0);
print_LCD_text(2, "e.g.:2100>", 0, 0, 1);
get_KEYPAD_input(4);
Set[2][3] = 1000*Data[2][1] + 100*Data[2][2] + 10*Data[2][3] + Data[2][4];
Mode[0] = 32;
}

void read_HX711_input() {
Data[0][3] = HX711.get_units(10);
Data[1][3] = Set[1][3]*(Data[0][3] - Set[0][3])/Set[2][3];
print_LCD_text(2, "Raw data:  ", 0, 0, 0);
print_LCD_text(1, "", Data[0][3], 11, 0);
print_LCD_text(2, "Real data: ", 0, 0, 1);
print_LCD_text(1, "", Data[1][3], 11, 1);
}

void set_ANALOG_cal() {
Set[0][4] = analogRead(Pin_ANALOG_Read);
print_LCD_text(2, "Raw Data1: ", 0, 0, 0);
print_LCD_text(1, "", Set[0][4], 11, 0);
print_LCD_text(2, "Cal Data1:", 0, 0, 1);
get_KEYPAD_input(4);
Set[1][4] = 1000*Data[2][1] + 100*Data[2][2] + 10*Data[2][3] + Data[2][4];
Set[2][4] = analogRead(Pin_ANALOG_Read);
print_LCD_text(2, "Raw Data2: ", 0, 0, 0);
print_LCD_text(1, "", Set[2][4], 11, 0);
print_LCD_text(2, "Cal Data2: ", 0, 0, 1);
get_KEYPAD_input(4);
Set[3][4] = 1000*Data[2][1] + 100*Data[2][2] + 10*Data[2][3] + Data[2][4];
Mode[0] = 42;
}

void read_ANALOG_input() {
Data[0][4] = analogRead(Pin_ANALOG_Read);
Data[1][4] = Set[1][4] + (Data[0][4] - Set[0][4])*(Set[3][4] - Set[1][4])/(Set[2][4] - Set[0][4]);
print_LCD_text(2, "Raw data:  ", 0, 0, 0);
print_LCD_text(1, "", Data[0][4], 11, 0);
print_LCD_text(2, "Real data: ", 0, 0, 1);
print_LCD_text(1, "", Data[1][4], 11, 1);
}

void read_I2C_input() {
Wire.beginTransmission(0x68);
Wire.write(0x3B);
Wire.endTransmission();
Wire.requestFrom(0x68,14);
while(Wire.available() < 14);
Data[0][8] = Wire.read()<<8|Wire.read();
Data[0][9] = Wire.read()<<8|Wire.read();
Data[0][10] = Wire.read()<<8|Wire.read();
Data[0][11] = Wire.read()<<8|Wire.read();
Data[0][5] = Wire.read()<<8|Wire.read();
Data[0][6] = Wire.read()<<8|Wire.read();
Data[0][7] = Wire.read()<<8|Wire.read();
Data[1][5] = (Data[0][5] - Set[0][5]) * Set[1][5] / Set[2][5];
Data[1][6] = (Data[0][6] - Set[0][6]) * Set[1][6] / Set[2][6];
Data[1][7] = (Data[0][7] - Set[0][7]) * Set[1][7] / Set[2][7];
Data[1][8] = (Data[0][8] - Set[0][8]) * Set[1][8] / Set[2][8] / 1000;
Data[1][9] = (Data[0][9] - Set[0][9]) * Set[1][9] / Set[2][9] / 1000;
Data[1][10] = (Data[0][10] - Set[0][10]) * Set[1][10] / Set[2][10] / 1000;
Data[1][11] = (Data[0][11] - Set[0][11]) * Set[1][11] / Set[2][11];
print_LCD_text(2, "G:", 0, 0, 0);
print_LCD_text(1, "", Data[0][5], 2, 0);
print_LCD_text(2, " ", 0, 6, 0);
print_LCD_text(1, "", Data[0][6], 7, 0);
print_LCD_text(2, " ", 0, 11, 0);
print_LCD_text(1, "", Data[0][7], 12, 0);
print_LCD_text(2, "g:", 0, 0, 1);
print_LCD_text(1, "", Data[1][5], 2, 1);
print_LCD_text(2, " ", 0, 6, 1);
print_LCD_text(1, "", Data[1][6], 7, 1);
print_LCD_text(2, " ", 0, 11, 1);
print_LCD_text(1, "", Data[1][7], 12, 1);
delay(1000);
print_LCD_text(2, "A:", 0, 0, 0);
print_LCD_text(1, "", Data[0][8], 2, 0);
print_LCD_text(2, " ", 0, 6, 0);
print_LCD_text(1, "", Data[0][9], 7, 0);
print_LCD_text(2, " ", 0, 11, 0);
print_LCD_text(1, "", Data[0][10], 12, 0);
print_LCD_text(2, "a:", 0, 0, 1);
print_LCD_text(1, "", Data[1][8], 2, 1);
print_LCD_text(2, " ", 0, 6, 1);
print_LCD_text(1, "", Data[1][9], 7, 1);
print_LCD_text(2, " ", 0, 11, 1);
print_LCD_text(1, "", Data[1][10], 12, 1);
delay(1000);
}

void write_ANALOG_output() {
Data[0][12] = analogRead(Pin_KNOB_SIG);
Data[1][12] = (Data[0][12] - Set[0][12]) * Set[1][12] / Set[2][12];
analogWrite(Pin_ANALOG_Write, Data[1][12]);
print_LCD_text(2, "Input  : ", 0, 0, 0);
print_LCD_text(1, "", Data[0][12], 10, 0);
print_LCD_text(2, "Output : ", 0, 0, 1);
print_LCD_text(1, "", Data[1][12], 10, 1);
}

void read_DIGITAL_input() {
Data[0][13] = digitalRead(Pin_DIGITAL_Read);
print_LCD_text(2, "Raw data: ", 0, 0, 0);
print_LCD_text(1, "", Data[0][13], 10, 0);
}

void write_DIGITAL_output() {
for (int j = 0; j < 100; j++) {
digitalWrite(Pin_DIGITAL_Write, LOW);
delay(500);
digitalWrite(Pin_DIGITAL_Write, HIGH);
delay(500);
}}