#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include "HX711.h"

#define Pin_LDR_AO 19
#define Pin_LDR_DO 18
#define Pin_RC_SIG 34
#define Pin_SERVO_PWM 23
#define Pin_HX711_SCK 17
#define Pin_HX711_DT 5
#define Pin_VOLT_SIG 35
#define Pin_AMP_SIG 32

LiquidCrystal_I2C LCD(0x27,16,2); // 0x3F
HX711 HX711;

// Speed of ultrasonic wave: Set[0][2]
// MPU6050: Set[1][1] to Set[1][14]
// Servo control: Set[2][2] to Set[2][3]
// HX711: Set[3][1] to Set[3][4]
// LDR: Set[3][5] to Set[3][7]
// VOLT: Set[3][8] to Set[3][11]
// AMP: Set[3][12] to Set[3][15]
int Bit[8] = {0, 0, 0, 0, 0, 0, 0, 0};
float Set[5][16] = {
{0, 0, 343, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
{0, 0, 0, 0, 0, 0, 0, 0, 15.27, 15.27, 15.27, 2.394, 2.394, 2.394, 0, 0},
{0, 0, 0.3, 0.25, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
{0, 0, 2100, 0, 5000, 0, 0, 0, 0, 4095, 1000, 2000, 0, 4095, 1000, 2000},
{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}};
float 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}};

void setup() {
Wire.begin();
Serial.begin(9600);
PIN_set();
LCD_start();
I2C_start();
I2C_set();
LEDC_set();
HX711.begin(Pin_HX711_DT, Pin_HX711_SCK);}

void loop() {
RC_read();
SERVO_write();
HX711_read();
LDR_read();
VOLT_read();
AMP_read();
// MPU6050_read();
DATA_show();}

void PIN_set() {
pinMode(Pin_LDR_DO, INPUT);
pinMode(Pin_LDR_AO, INPUT);
pinMode(Pin_RC_SIG, INPUT);
pinMode(Pin_VOLT_SIG, INPUT);
pinMode(Pin_AMP_SIG, INPUT);
pinMode(Pin_SERVO_PWM, OUTPUT);}

void LCD_start() {
LCD.init();
LCD.clear();
LCD.backlight();
LCD_print(4, "LCD ", 0, 6, 0);
LCD_print(4, "Starting >", 0, 6, 1);
for (int i = 0; i <= 3; i++) {
LCD_print(1, "", 3 - i, 15, 1);
delay(990);}
LCD_print(6, "", 0, 0, 0);}

void LCD_print(int type, String text, float value, int col, int row) {
LCD.display();
LCD.setCursor(col, row);
switch (type) {
case 1: LCD.println(value, DEC); break;
case 2: LCD.println(value, HEX); break;
case 3: LCD.clear(); LCD.println(value); break;
case 4: LCD.println(text); break;
case 5: LCD.clear(); LCD.println(text); break;
case 6: LCD.clear(); LCD.noDisplay();}}

void I2C_start() {
Wire.begin();
LCD_print(5, "I2C scanning ...", 0, 0, 0);
delay(2000);
for (byte j = 8; j < 120; j++) {
Wire.beginTransmission (j);
if (Wire.endTransmission () == 0) {
LCD_print(4, "Addr :", 0, 0, 1);
LCD_print(1, "", j, 6, 1);
LCD_print(4, " (0x", 0, 9, 1);
LCD_print(2, "", j, 13, 1);
LCD_print(4, ")", 0, 15, 1);
delay(2000);}}
LCD_print(6, "", 0, 0, 0);}

void I2C_set() {
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 LEDC_set() {
// ledcAttach(pin[#], freq[Hz], res[bit])
ledcAttach(Pin_SERVO_PWM, 50, 13);}

void RC_read() {
// Data[2][2] = pulseIn(Pin_RC_SIG, HIGH);
// for servo, 508 us (0 deg); 2500 us (180 deg)
// for motor, 1000 us (0 speed); 2000 us (max speed)
Data[2][1] = analogRead(Pin_RC_SIG);
Data[2][2] = map(Data[2][1], 0, 4095, 1000, 2000);}

void SERVO_write() {
Data[2][3] = map(Data[2][2], 0, 20000, 0, 8191);
ledcWrite(Pin_SERVO_PWM, Data[2][3]);}

void HX711_read() {
Data[3][1] = HX711.get_units(10);
Data[3][2] = map(Data[3][1], Set[3][1], Set[3][2], Set[3][3], Set[3][4]);}

void LDR_read() {
if (digitalRead(Pin_LDR_DO) == LOW) {
Data[3][3] = micros();
Data[3][5] = 0.000001*(Data[3][3] - Data[3][4]);}
else {
Data[3][4] = micros();
Data[3][6] = 0.000001*(Data[3][4] - Data[3][3]);}
Data[3][7] = 60.0/(Data[3][5] + Data[3][6]); //rpm
Data[3][8] = analogRead(Pin_LDR_AO);
Data[3][9] = map(Data[3][8], 0, Set[3][5], Set[3][6], Set[3][7]);}

void VOLT_read() {
Data[3][10] = analogRead(Pin_VOLT_SIG);
Data[3][11] = map(Data[3][10], Set[3][8], Set[3][9], Set[3][10], Set[3][11]);}

void AMP_read() {
Data[3][12] = analogRead(Pin_AMP_SIG);
Data[3][13] = map(Data[3][12], Set[3][12], Set[3][13], Set[3][14], Set[3][15]);}

void MPU6050_read() {
Wire.beginTransmission(0x68);
Wire.write(0x3B);
Wire.endTransmission();
Wire.requestFrom(0x68,14);
while(Wire.available() < 14);
Data[1][4] = Wire.read()<<8|Wire.read();
Data[1][5] = Wire.read()<<8|Wire.read();
Data[1][6] = Wire.read()<<8|Wire.read();
Data[1][7] = Wire.read()<<8|Wire.read();
Data[1][1] = Wire.read()<<8|Wire.read();
Data[1][2] = Wire.read()<<8|Wire.read();
Data[1][3] = Wire.read()<<8|Wire.read();
Data[1][8] = (Data[1][1] - Set[1][1]) * Set[1][8]/1000;
Data[1][9] = (Data[1][2] - Set[1][2]) * Set[1][9]/1000;
Data[1][10] = (Data[1][3] - Set[1][3]) * Set[1][10]/1000;
Data[1][11] = (Data[1][4] - Set[1][4]) * Set[1][11]/1000;
Data[1][12] = (Data[1][5] - Set[1][5]) * Set[1][12]/1000;
Data[1][13] = (Data[1][6] - Set[1][6]) * Set[1][13]/1000;
Data[1][14] = (Data[1][7] - Set[1][7]) * Set[1][14]/1000;}

void DATA_show() {
LCD_print(5, "F", 0, 0, 0);
LCD_print(1, "", Data[3][2], 1, 0);
LCD_print(4, "R", 0, 8, 0);
LCD_print(1, "", Data[3][7], 9, 0);
LCD_print(4, "V", 0, 0, 1);
LCD_print(1, "", Data[3][11], 1, 1);
LCD_print(4, "A", 0, 8, 1);
LCD_print(1, "", Data[3][13], 9, 1);
/*
LCD_print(4, "G:", 0, 0, 0);
LCD_print(1, "", Data[1][8], 2, 0);
LCD_print(4, " ", 0, 6, 0);
LCD_print(1, "", Data[1][9], 7, 0);
LCD_print(4, " ", 0, 11, 0);
LCD_print(1, "", Data[1][10], 12, 0);
LCD_print(4, "A:", 0, 0, 1);
LCD_print(1, "", Data[1][11], 2, 1);
LCD_print(4, " ", 0, 6, 1);
LCD_print(1, "", Data[1][12], 7, 1);
LCD_print(4, " ", 0, 11, 1);
LCD_print(1, "", Data[1][13], 12, 1);
delay(1000);
*/}