#include "Keypad.h" // khai bao thư vien keypad
#include "Servo.h"
#include "ArduinoSTL.h"
#include "Adafruit_GFX.h"       // khai bao thư vien Adafruit graphics library
#include "Adafruit_ILI9341.h"   // khai bao thư vien Adafruit ILI9341 TFT library

#include "Vector.h"
#include "map"
#define TFT_CS 8 // khai bao chan cs
#define TFT_RST 9 // khai bao chan reset
#define TFT_DC 53 // khai bao chan dc

// -------------------------LCD Setup-----------------------------------------
// initialize ILI9341 TFT library
Adafruit_ILI9341 tft = Adafruit_ILI9341(TFT_CS, TFT_DC, TFT_RST);

// -------------------------Keypad Setup---------------------------------------
const byte ROWS = 4; // Four Rows
const byte COLS = 4; // Four Columns
char keys[ROWS][COLS] = {
    {'7','8','9','A'},
    {'4','5','6','B'},
    {'1','2','3','C'},
    {'*','0','#','D'}
};
const int MOTOR_PIN_SIZE = 12;
const int MOTOR_PINS[13] = {-1, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33};

Servo myservo;
int degree = 0;

String key_tittle_mapping[16][2] = {
    {"7", "START"},
    {"8", "RESET"},
    {"9", "<"},
    {"A", ">"},
    {"4", "BACK RISE"},
    {"5", "BACK DOWN"},
    {"6", "^"},
    {"B", "v"},
    {"1", "SIT UP"},
    {"2", "LEG DOWN"},
    {"3", "LEFT TURN"},
    {"C", "RIGHT TURN"},
    {"*", "BEDPAN ON"},
    {"0", "BEDPAN OFF"},
    {"#", "AUTO TURN A"},
    {"D", "AUTO TURN B"},
};

std::map <char, std::Vector <std::pair <std::string, std::Vector <int>>>> motor_movement_mapping = {
    {'8', {
        {"pin", {LOW, HIGH, LOW, HIGH, LOW, HIGH, LOW, HIGH, LOW, HIGH, HIGH, HIGH}},
        {"delay", {3000}},
        {"pin_reset_high", {}}
    }},
    {'9', {
        {"pin_reset_high", {}},
        {"degree_add", {10}}
    }},
    {'A', {
        {"pin_reset_high", {}},
        {"degree", {150}},
        {"delay", {15}}
    }},
    {'4', {
        {"pin", {-1, HIGH, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}}
    }},
    {'5', {
        {"pin", {HIGH, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}}
    }},
    {'6', {
        {"pin", {HIGH, HIGH, HIGH, HIGH, HIGH, HIGH, HIGH, HIGH, HIGH, HIGH, LOW, HIGH}},
        {"delay", {250}},
        {"pin_reset_high", {}}
    }},
    {'B', {
        {"pin", {HIGH, HIGH, HIGH, HIGH, HIGH, HIGH, HIGH, HIGH, HIGH, HIGH, HIGH, LOW}},
        {"delay", {250}},
        {"pin_reset_high", {}}
    }},
    {'1', {
        {"pin", {-1, HIGH, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}}
    }},
    {'2', {
        {"pin", {HIGH, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}}
    }},
    {'3', {
        {"pin", {-1, HIGH, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}}
    }},
    {'C', {
        {"pin", {HIGH, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}}
    }},
    {'*', {
        {"pin", {-1, HIGH, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}}
    }},
    {'0', {
        {"pin", {HIGH, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}}
    }},
    {'#', {
        {"pin", {-1, HIGH, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}}
    }},
    {'D', {
        {"pin", {HIGH, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}}
    }},
};

byte rowPins[ROWS] = {A8, A9, A10, A11}; // Arduino pins connected to the row pins of the keypad
byte colPins[COLS] = {A12, A13, A14, A15}; // Arduino pins connected to the column pins of the keypad
Keypad keypad_key = Keypad(makeKeymap(keys), rowPins, colPins, ROWS, COLS); // Keypad Library definition

int find_str(String str, char find_chr){
    for (int pos=0; pos<str.length(); pos++){
        if (str[pos] == find_chr){
            return pos;
        }
    }
    return -1;
}

String get_key_tittle_mapping(char key){
    String str_key = String(key);

    for (int i=0; i<ROWS * COLS; i++){
        if (key_tittle_mapping[i][0] == str_key){
            return key_tittle_mapping[i][1];
        }
    }
    return "";
}

void get_pos(char key, int &pos_x, int &pos_y) {  // get (x, y)
    for (int col=1; col <= COLS; col++){
        for (int row=1; row <= ROWS; row++){
            if (keys[row - 1][col - 1] == key){
                pos_x = col;
                pos_y = row;
                return;
            }
        }
    }

    pos_x = 0;
    pos_y = 0;
}

void draw_button(String tittle, int pos_x, int pos_y, int color = 0){
    pos_x = 15 + (pos_x - 1) * 45;
    pos_y = 40 + (pos_y - 1) * 45;

    uint16_t button_color;
    if (color == 0) { // green
        button_color = tft.color565(64, 237, 39);
    }
    else { // red
        button_color = tft.color565(238, 38, 91);
    }
    int pos_first_space = find_str(tittle, ' ');
    if (pos_first_space != -1) {
        String word_up = tittle.substring(0, pos_first_space);
        String word_down = tittle.substring(pos_first_space + 1);

        tft.fillRect(pos_x, pos_y, 40, 40, button_color);
        tft.setCursor(pos_x + 3, pos_y + 10);
        tft.setTextColor(tft.color565(0, 0, 0));
        tft.setTextSize(1);
        tft.println(word_up);

        tft.setCursor(pos_x + 3, pos_y + 20);
        tft.setTextColor(tft.color565(0, 0, 0));
        tft.setTextSize(1);
        tft.println(word_down);
    }
    else {
        int text_size = 1;

        if (tittle == ">" || tittle == "<" || tittle == "v") {
            text_size = 3;
        }
        else if (tittle == "^"){
            text_size = 4;
        }
        tft.fillRect(pos_x, pos_y, 40, 40, button_color);
        tft.setCursor(pos_x + 5, pos_y + 15);
        tft.setTextColor(tft.color565(0, 0, 0));
        tft.setTextSize(text_size);
        tft.println(tittle);
    }
}

void poweron_screen() {
    tft.fillRect(0, 0, 320, 240, tft.color565(200,200, 200));
    tft.fillRoundRect(20, 20, 280, 200, 20, tft.color565(243,87, 35));
    tft.setCursor(90, 90);
    tft.setTextColor(tft.color565(255, 255, 255));
    tft.setTextSize(8);
    tft.println("M2D ");
}

void startup_screen() {
    tft.fillRect(0, 0, 320, 240, tft.color565(164,255, 255));
    tft.setCursor(50, 80);
    tft.setTextColor(tft.color565(0, 164, 164));
    tft.setTextSize(3);
    tft.println("GIUONG HO TRO ");
    tft.setCursor(40, 110);
    tft.setTextColor(tft.color565(243, 87, 35));
    tft.setTextSize(3);
    tft.println("NGUOI TAI BIEN");
}

void init_screen(){
    tft.fillScreen(tft.color565(0, 0, 0));

    tft.fillRect(10, 10, 300, 220, tft.color565(116, 139, 248));
    tft.setCursor(20, 20);
    tft.setTextColor(tft.color565(159, 30, 6));
    tft.setTextSize(2);
    tft.println("MAIN CONTROL");

    tft.drawLine(230, 130, 200, 150, ILI9341_WHITE);
    tft.drawLine(230, 130, 270, 130, ILI9341_WHITE);
    tft.drawLine(270, 130, 300, 100, ILI9341_WHITE);
}

void reset_screen(){
    for (int col=1; col <= COLS; col++){
        for (int row=1; row <= ROWS; row++){
            String tittle = get_key_tittle_mapping(keys[row - 1][col - 1]);
            draw_button(tittle, col, row, 0);
        }
    }
}

void handle_motor(char key){
    std::Vector <std::pair <std::string, std::Vector <int>>> motor_steps = motor_movement_mapping[key];

    for (std::pair <std::string, std::Vector <int>> step : motor_steps) {
        if (step.first == "pin") {
            for (int pin_no = 1; pin_no <= MOTOR_PIN_SIZE; pin_no++){
                int signal = step.second[pin_no - 1];

                if (signal != -1){
                    digitalWrite(MOTOR_PINS[pin_no], signal);
                }
            }
        }
        else if (step.first == "pin_reset_high") {
            for (int pin_no = 1; pin_no <= MOTOR_PIN_SIZE; pin_no++){
                digitalWrite(MOTOR_PINS[pin_no], HIGH);
            }
        }
        else if (step.first == "delay") {
            int delay_time = step.second[0];
            delay(delay_time);
        }
        else if (step.first == "degree_add") {
            int degree_add_value = step.second[0];
            degree += degree_add_value;
        }
        else if (step.first == "degree") {
            int degree_new_value = step.second[0];
            degree = degree_new_value;
        }
    }
}

void make_red_start_button(){
    char key = '7';
    int pos_x = 0;
    int pos_y = 0;
    get_pos(key, pos_x, pos_y);
    String tittle = get_key_tittle_mapping(key);
    draw_button(tittle, pos_x, pos_y, 1);
}

void setup() {
    for (int pin_no = 1; pin_no <= MOTOR_PIN_SIZE; pin_no++){
        pinMode(MOTOR_PINS[pin_no], OUTPUT);
    }

    tft.begin();
    tft.setRotation(3);
    bool is_init_screen = false;
    poweron_screen();

    while (!is_init_screen){
        char key = keypad_key.waitForKey();
        if (key != NO_KEY){
            if (key == '7'){
                startup_screen();
                delay(1000);
                init_screen();
                is_init_screen = true;
                int pos_x = 0;
                int pos_y = 0;
                get_pos(key, pos_x, pos_y);
                String tittle = get_key_tittle_mapping(key);
                reset_screen();
                draw_button(tittle, pos_x, pos_y, 1);
            }
        }

        myservo.attach(11, 500, 2500);
    }
}

void loop() {
    char key = keypad_key.waitForKey();

    if (key != NO_KEY){
        int pos_x = 0;
        int pos_y = 0;
        get_pos(key, pos_x, pos_y);
        String tittle = get_key_tittle_mapping(key);
        reset_screen();
        make_red_start_button();
        draw_button(tittle, pos_x, pos_y, 1);

        myservo.write(degree);
        handle_motor(key);
    }
}