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

#define Pin_KNOB1_SIG 4
#define Pin_KNOB2_SIG 36
#define Pin_KNOB3_SIG 2
#define Pin_KNOB4_SIG 15
#define Pin_PWM1_Write 33
#define Pin_PWM2_Write 25
#define Pin_PWM3_Write 26
#define Pin_PWM4_Write 5
#define Pin_RC1_Read 32
#define Pin_RC2_Read 35
#define Pin_RC3_Read 34
#define Pin_RC4_Read 39
#define Pin_SERVO1_PWM 13
#define Pin_SERVO2_PWM 27
#define Pin_SERVO3_PWM 14
#define Pin_SERVO4_PWM 12

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_PWM1_Write, OUTPUT);
pinMode(Pin_PWM2_Write, OUTPUT);
pinMode(Pin_PWM3_Write, OUTPUT);
pinMode(Pin_PWM4_Write, OUTPUT);
pinMode(Pin_RC1_Read, INPUT);
pinMode(Pin_RC2_Read, INPUT);
pinMode(Pin_RC3_Read, INPUT);
pinMode(Pin_RC4_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)
for (int i = 0; i < 9; i++) {ledcSetup(i, 50, 10);}
ledcAttachPin(Pin_SERVO1_PWM, 1);
ledcAttachPin(Pin_SERVO2_PWM, 2);
ledcAttachPin(Pin_SERVO3_PWM, 3);
ledcAttachPin(Pin_SERVO4_PWM, 4);
ledcAttachPin(Pin_PWM1_Write, 5);
ledcAttachPin(Pin_PWM2_Write, 6);
ledcAttachPin(Pin_PWM3_Write, 7);
ledcAttachPin(Pin_PWM4_Write, 8);}

void KNOB_read() {
Data[0][1] = analogRead(Pin_KNOB1_SIG);
Data[0][2] = analogRead(Pin_KNOB2_SIG);
Data[0][3] = analogRead(Pin_KNOB3_SIG);
Data[0][4] = analogRead(Pin_KNOB4_SIG);}

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

void RC_read() {
Data[1][1] = pulseIn(Pin_RC1_Read, HIGH);
Data[1][2] = pulseIn(Pin_RC2_Read, HIGH);
Data[1][3] = pulseIn(Pin_RC3_Read, HIGH);
Data[1][4] = pulseIn(Pin_RC4_Read, HIGH);
Data[1][5] = pulseIn(Pin_RC1_Read, LOW);
Data[1][6] = pulseIn(Pin_RC2_Read, LOW);
Data[1][7] = pulseIn(Pin_RC3_Read, LOW);
Data[1][8] = pulseIn(Pin_RC4_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);}
if (Data[2][2] > 520) {
Din[2] = Data[2][3]; Din[3] = Data[2][3];
Din[1] = Data[2][3] + (Data[2][2] - 511)/3;
Din[4] = Data[2][3] + (Data[2][2] - 511)/3;}
if (Data[2][2] < 500) {
Din[1] = Data[2][3]; Din[4] = Data[2][3];
Din[2] = Data[2][3] - (Data[2][2] - 511)/3;
Din[3] = Data[2][3] - (Data[2][2] - 511)/3;}
if (Data[2][1] > 520) {
Din[1] = Data[2][3]; Din[2] = Data[2][3];
Din[3] = Data[2][3] + (Data[2][1] - 511)/3;
Din[4] = Data[2][3] + (Data[2][1] - 511)/3;}
if (Data[2][1] < 500) {
Din[3] = Data[2][3]; Din[4] = Data[2][3];
Din[1] = Data[2][3] - (Data[2][1] - 511)/3;
Din[2] = Data[2][3] - (Data[2][1] - 511)/3;}
else {
Din[1] = Data[2][3]; Din[2] = Data[2][3];
Din[3] = Data[2][3]; Din[4] = Data[2][3];}
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[1][1]/10, 0, 0);
LCD_print(1, "", Dout[1], 0, 1);
LCD_print(1, "", Data[1][2]/10, 4, 0);
LCD_print(1, "", Dout[2], 4, 1);
LCD_print(1, "", Data[1][3]/10, 8, 0);
LCD_print(1, "", Dout[3], 8, 1);
LCD_print(1, "", Data[1][4]/10, 12, 0);
LCD_print(1, "", Dout[4], 12, 1);}