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

#define Pin_HX711_DT 5
#define Pin_HX711_SCK 6
#define Pin_ANALOG1_SIG A0
#define Pin_ANALOG2_SIG A1
#define Pin_KNOB1_SIG A2
#define Pin_KNOB2_SIG A3
uint8_t Pin_SERVO1_PWM = 9;
uint8_t Pin_SERVO2_PWM = 10;
uint8_t Pin_KEYPAD_Row[4] = {13, 12, 11, 8};
uint8_t Pin_KEYPAD_Col[4] = {7, 4, 3, 2};

int Mode[8] = {0, 0, 0, 0, 0, 0, 0, 0};
char operation = 0;
double memory = 0;
String current = "";
double answer = 0;
double display = 0;
char Keys[4][4] = {
{'1', '2', '3', '+'},
{'4', '5', '6', '-'},
{'7', '8', '9', '*'},
{'.', '0', '=', '/'}};

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

void setup() {
pinMode(Pin_SERVO1_PWM, OUTPUT);
pinMode(Pin_SERVO2_PWM, OUTPUT);
Serial.begin(9600);
LCD_start();
PWM_set();
HX711.begin(Pin_HX711_DT, Pin_HX711_SCK);
Mode[0] = 10;
}

void loop() {
KEYPAD_get_option();
switch (Mode[0]) {
case 10: MCU_read_data(); break;
case 11: SERVO_read_data(); break;
case 12: HX711_send_data(); break;
}}

void LCD_start() {
LCD.init();
LCD.backlight();
LCD.clear();
LCD.cursor();
LCD.setCursor(0, 0);}

void PWM_set() {
// 0 deg -> 0.544 ms -> OCR 1088
// 180 deg -> 2.4 ms -> OCR 4800
// TCCR0 -> 5 & 6, TCCR1 -> 9 & 10, TCCR2 -> 11 & 3 
TCCR1A = _BV(COM1A1) | _BV(COM1B1) | _BV(WGM11);
TCCR1B = _BV(WGM13) | _BV(WGM12) | _BV(CS11);
ICR1 = 39999; // 20 ms period (50 Hz)
OCR1A = 3000; // 90 deg -> 1.5 ms -> OCR1A 3000
OCR1B = 3000; // 90 deg -> 1.5 ms -> OCR1B 3000
}

void KEYPAD_get_option() {
if (display == 123) {Mode[0] = 11; display = 0;}
if (display == 234) {Mode[0] = 12; display = 0;}}

void MCU_read_data() {
char key = KEYPAD.getKey();
if (key) {LCD.print(key);
if (key == '+' || key == '-' || key == '*' || key == '/') {
memory = current.toDouble(); current = "";
if (answer == 0) {answer = memory; operation = key;}
else {MCU_calculate(operation, answer, memory); operation = key;}}
else if (key == '=') {if (display == 0) {
memory = current.toDouble();
MCU_calculate(operation, answer, memory); display = answer;
LCD.setCursor(0, 1); LCD.print(display);}
else {memory = 0; current = ""; answer = 0; display = 0;
LCD.clear(); LCD.setCursor(0, 0);}}
else {current += String(key);}}}

void MCU_calculate(char operation, double left, double right) {
switch (operation) {
case '+': answer = left + right; break;
case '-': answer = left - right; break;
case '*': answer = left * right; break;
case '/': answer = left / right; break;}}

void SERVO_read_data() {
int knob1 = analogRead(Pin_KNOB1_SIG);
int knob2 = analogRead(Pin_KNOB2_SIG);
int servo1 = map(knob1, 0, 1023, 1088, 4800);
int servo2 = map(knob2, 0, 1023, 1088, 4800);
OCR1A = servo1;
OCR1B = servo2;}

void HX711_send_data() {
int load = HX711.get_units();
float weight = load * 5.0 / 2100;
LCD.setCursor(0, 0);
LCD.print("Raw:");
LCD.setCursor(8, 0);
LCD.print(load);
LCD.setCursor(0, 1);
LCD.print("Weight:");
LCD.setCursor(8, 1);
LCD.print(weight);
delay(500);
LCD.clear();}