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

#define Pin_RC1_Read 12
#define Pin_RC2_Read 2
#define Pin_RC3_Read 4
#define Pin_RC4_Read 7
#define Pin_KNOB1_SIG A1
#define Pin_KNOB2_SIG A2
#define Pin_KNOB3_SIG A3
#define Pin_KNOB4_SIG A0
uint8_t Pin_SERVO1_PWM = 10;
uint8_t Pin_SERVO2_PWM = 3;
uint8_t Pin_SERVO3_PWM = 5;
uint8_t Pin_SERVO4_PWM = 9;

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_SERVO1_PWM, OUTPUT);
pinMode(Pin_SERVO2_PWM, OUTPUT);
pinMode(Pin_SERVO3_PWM, OUTPUT);
pinMode(Pin_SERVO4_PWM, OUTPUT);
Serial.begin(9600);
LCD_start();
I2C_start();
PWM_set();}

void loop() {
KNOB_read(); // 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.backlight();
LCD.clear();
//LCD.cursor();
LCD.setCursor(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 PWM_set() {
// 20 ms (50 Hz) -> ICR1 40000 -> OCR0/2A 156
// 0 deg -> 0.5 ms -> OCR1 1000 -> OCR0/2B 3.9 (4)
// 180 deg -> 2.4 ms -> OCR1 4800 -> OCR0/2B 18.72 (19)
// Zero throttle -> 1 ms -> OCR1 2000 -> OCR0/2B 7.8 (8)
// Full throttle -> 2 ms -> OCR1 4000 -> OCR0/2B 15.6 (16)
/* ----------------------------------------------------
| TCCR0 -> 5 & 6, TCCR1 -> 9 & 10, TCCR2 -> 11 & 3    |
| TCCR0A: COM0A1 COM0A0 COM0B1 COM0B0 – – WGM01 WGM00 |
| TCCR0B: FOC0A FOC0B – – WGM02 CS02 CS01 CS00        |
| TCCR1A: COM1A1 COM1A0 COM1B1 COM1B0 – – WGM11 WGM10 |
| TCCR1B: ICNC1 ICES1 – WGM13 WGM12 CS12 CS11 CS10    |
| TCCR2A: COM2A1 COM2A0 COM2B1 COM2B0 – – WGM21 WGM20 |
| TCCR2B: FOC2A FOC2B – – WGM22 CS22 CS21 CS20        |
---------------------------------------------------- */
ICR1 = 40000;
OCR0A = 156;
OCR0B = 5;
OCR2A = 156;
OCR2B = 5;
TCCR1A = 0b10100010;
TCCR1B = 0b00011010;
TCCR0A = 0b01100001;
TCCR0B = 0b00001101;
TCCR2A = 0b01100001;
TCCR2B = 0b00001111;}

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 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];}
*/
if (Data[0][2] > 520) {
Din[2] = Data[0][3]; Din[3] = Data[0][3];
Din[1] = Data[0][3] + (Data[0][2] - 511)/3;
Din[4] = Data[0][3] + (Data[0][2] - 511)/3;}
if (Data[0][2] < 500) {
Din[1] = Data[0][3]; Din[4] = Data[0][3];
Din[2] = Data[0][3] - (Data[0][2] - 511)/3;
Din[3] = Data[0][3] - (Data[0][2] - 511)/3;}
if (Data[0][1] > 520) {
Din[1] = Data[0][3]; Din[2] = Data[0][3];
Din[3] = Data[0][3] + (Data[0][1] - 511)/3;
Din[4] = Data[0][3] + (Data[0][1] - 511)/3;}
if (Data[0][1] < 500) {
Din[3] = Data[0][3]; Din[4] = Data[0][3];
Din[1] = Data[0][3] - (Data[0][1] - 511)/3;
Din[2] = Data[0][3] - (Data[0][1] - 511)/3;}
else {
Din[1] = Data[0][3]; Din[2] = Data[0][3];
Din[3] = Data[0][3]; Din[4] = Data[0][3];}
Dout[1] = map(Din[1], 0, 1023, 1000, 4800);
Dout[2] = map(Din[2], 0, 1023, 4, 19);
Dout[3] = map(Din[3], 0, 1023, 4, 19);
Dout[4] = map(Din[4], 0, 1023, 1000, 4800);}

void SERVO_write() {
OCR0A = 156;
OCR2A = 156;
OCR0B = Dout[3]; /*pin 5*/
OCR2B = Dout[2]; /*pin 3*/
OCR1A = Dout[4]; /*pin 9*/
OCR1B = Dout[1]; /*pin 10*/}

void LCD_show() {
LCD_print(1, "", Data[1][1]/10, 0, 0);
LCD_print(1, "", Dout[1]/256, 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]/256, 12, 1);}