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

#define Pin_KNOB2_SIG 2
#define Pin_PWM2_Write 25
#define Pin_RC2_Read 35
#define Pin_SERVO2_PWM 27

int Din[9] = {0, 0, 0, 0, 0, 0, 0, 0, 0};
int Dout[9] = {0, 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}};

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

void setup() {
pinMode(Pin_PWM2_Write, OUTPUT);
pinMode(Pin_RC2_Read, INPUT);
Serial.begin(9600);
LCD_start();
I2C_start();
LEDC_set();}

void loop() {
KNOB_read(); // remove
PWM_write(); // remove
RC_read();
DATA_process();
SERVO_write();
LCD_show();}

void LCD_print(int type, String text, byte 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 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 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 LEDC_set() {
// ledcSetup(Channel, Frequency, Resolution)
ledcAttachChannel(Pin_SERVO2_PWM, 50, 10, 2);
ledcAttachChannel(Pin_PWM2_Write, 50, 10, 6);}

void KNOB_read() {
Data[0][2] = analogRead(Pin_KNOB2_SIG);}

void PWM_write() {
// speed 26 = 508 us (0 deg); speed 123 = 2404 us (180 deg)
for (int i = 1; i < 4; i++) {Data[3][i] = map(Data[0][i], 0, 255, 26, 123);}
for (int i = 1; i < 4; i++) {ledcWrite(i + 4, Data[3][i]);}}

void RC_read() {
Data[1][2] = pulseIn(Pin_RC2_Read, HIGH);
Data[1][6] = pulseIn(Pin_RC2_Read, LOW);
for (int i = 1; i < 5; i++) {Data[1][i + 8] = Data[1][i] + Data[1][i + 4];}}

void DATA_process() {
for (int i = 1; i < 5; i++) {Data[2][i] = map(Data[1][i], 1000, 2000, 0, 1023);}
Din[2] = Data[2][2];
for (int i = 1; i < 5; i++) {Dout[i] = map(Din[i], 0, 1023, 26, 123);}}

void SERVO_write() {
for (int i = 1; i < 5; i++) {ledcWrite(i, Dout[i]);}}

void LCD_show() {
LCD_print(1, "", Data[0][2], 0, 0);
LCD_print(1, "", Data[3][2], 5, 0);
LCD_print(1, "", Data[1][6], 10, 0);
LCD_print(1, "", Data[2][2], 0, 1);
LCD_print(1, "", Din[2], 5, 1);
LCD_print(1, "", Dout[2], 10, 1);}