File content as of revision 0:a8b6c10f03dc:

#include "mbed.h"
#include "EthernetInterface.h"
#include <string>
#include <vector>
#include "Audio.h"
#include "iButton.h"
#include <bitset>
#include "CRC16Modbus.h"
#include "BufferedSoftSerial.h"
#include "Displays.h"
//#include <list>
//#include <algorithm>
#include <Watchdog.h>

// Network interface
TCPSocket socket;

Thread thd_Cek_Ibutton;
Thread thd_Buzzer;
Watchdog &watchdog = Watchdog::get_instance();

MyAudio buzz(D7);
Displays dsp(PTC8,PTC1);
int LCD_Step = 0;
int LCD_Step_4 = 0;

#define IP         "192.168.0.5"
#define GATEWAY    "192.168.0.1"
#define MASK       "255.255.255.0"

#define IPSERVER   "192.168.0.150"
#define PORTSERVER 12345

#define board_sum  2

char cmd_resets[] = "R00";
char cek_con[] = "PING";

int Key_On_Process = 0;

Serial dbg(USBTX,USBRX);
BufferedSoftSerial dv(PTB19,PTB18);
DigitalOut rede(PTC9,0);
iButton ibutton(D4);

bool tap = false;
bool lasttap = false;

int board_number;
uint8_t baris;
uint8_t kolom;
uint8_t real_id;
uint8_t real_blink_id[5] = {64,64,64,64,64};
uint8_t blinking_now = 0;
uint8_t idle_wait_result = 0;
uint8_t ids;

typedef uint8_t byte;
//r0,r1,r2,r3,r4,r5,r6,r7
byte Row_Board_Last[5][8],Row_Board_Now[5][8],Row_Board_Predict[5][8];

int board_88[8][8]= {
    {0,1,2,3,4,5,6,7},
    {8,9,10,11,12,13,14,15},
    {16,17,18,19,20,21,22,23},
    {24,25,26,27,28,29,30,31},
    {32,33,34,35,36,37,38,39},
    {40,41,42,43,44,45,46,47},
    {48,49,50,51,52,53,54,55},
    {56,57,58,59,60,61,62,63}
};

unsigned char buffer_crc[2];
unsigned short _crc;

byte respon_from_slave_temp[17];

bool intr_Done = false;

struct Frame_Data_Received {
    int ids;
    string ibutton;
    int _key_id;
    int cmd_id;
} frm_data_received;

byte respon_from_slave[32];

uint8_t step = 0;
bool isConnected = false;

vector<int> salah_taruh;
vector<int> salah_ambil;
vector<int> salah_taruh_last;
vector<int> salah_ambil_last;
int salah_ambil_count;
int salah_taruh_count;

bool done[5] = {true,true,true,true,true};

vector<int> lst_salah;
vector<int> lst_salah_hst;
vector<int>::iterator it;
vector<int> to_remove;

vector<uint8_t> to_sends;
vector<uint8_t> to_sends_last;

string ibutton_code;
vector<string> split(const string str, const string separator)
{
    char *cstr = const_cast<char*>(str.c_str());
    char *current;

    std::vector<std::string>arr;
    current = strtok(cstr, separator.c_str());
    while(current != NULL) {
        arr.push_back(current);
        current = strtok(NULL, separator.c_str());
    }
    return arr;
}

void Cek_Ibutton();
void Cek_Salah_Buzzer();
string Byte_to_StringBinary(byte data_byte);
uint8_t StringBinary_to_Byte(string data_binary);
int Get_Row(int key_id);
unsigned short Get_CRC_Modbus(byte data_byte[]);
void Send_RS485(int board,int key_id,int key_id_blinking,bool Cek_Respon);
void Make_Prediction(int board_number,int baris, int kolom);

void First_Update();
vector<string> temp_data;
int res_cmp = 0;
string bin_string_now;
string bin_string_predict;
int keys;
bool First_Run = true;
bool dataButton = false;

uint8_t ping_counter1 = 0;
uint8_t ping_counter2 = 0;
uint8_t ping_counter3 = 0;
uint8_t ping_counter4 = 0;

void FindingServer()
{
    printf("Finding Server.....\r\n\r\n");

RECONNECT1:
    EthernetInterface net;
    net.set_network(IP,MASK,GATEWAY);
    int stat1 = net.connect();
    //printf("stat1 = %d\r\n",stat1);
    if(stat1 != 0) {
        goto RECONNECT1;
    }
    // Show the network address
    const char *ip = net.get_ip_address();

    while(1) {
RECONNECT2:
        // Open a socket on the network interface, and create a TCP connection to mbed.org
        TCPSocket socket;
        socket.open(&net);
        socket.set_blocking(false);
        int stat2 = socket.connect(IPSERVER, PORTSERVER);
        //printf("stat2 = %d\r\n",stat2);
        wait(1);
        if(stat2 != 0) {
            socket.close();
            goto RECONNECT2;
        }
        watchdog.start(5000);
        isConnected = true;
        printf("Connected to Server....\r\n");

        if(First_Run) {
            printf("Process Start..\r\n");
            //socket.send(cmd_resets, sizeof(cmd_resets));

            First_Update();

            for(int i = 0; i<to_sends.size(); i++) {
                printf("%02X ", to_sends[i]);
            }

            socket.send(to_sends.data(), to_sends.size());
            First_Run = false;
            wait(0.05);
            dsp.step0();
            printf("Send First Update Complete\r\n");
        } else {
            if(blinking_now == 0) {
                //printf("Process Start..\r\n");
                //socket.send(cmd_resets, sizeof(cmd_resets));

                First_Update();
                step = 0;
                if(frm_data_received.ids == 3) {
                    dsp.step4(to_string(frm_data_received._key_id).c_str(),false);
                } else if(frm_data_received.ids== 1) {
                    dsp.step0();
                } else {
                    dsp.step0();
                }
                //socket.send(reinterpret_cast<char*> (&to_sends[0]), to_sends.size());
                socket.send(to_sends.data(), to_sends.size());

            } else {
                dsp.step5(to_string(blinking_now).c_str(),true);
            }
        }

        //printf("%s\r\n",data_frame.c_str());
        while(isConnected) {
            watchdog.kick();
            //TODO : CekData Locker Terkini
            
            ibutton.DetectiButton();
            if(ibutton.IsTaping())
                tap = true;
            else
                tap = false;
    
            if(tap != lasttap) {
                lasttap = tap;
                if(tap) {    
                    ibutton_code.clear();
                    ibutton_code = ibutton.GetData();
                    if((dataButton.length()== 12) && (dataButton == false)){
                        dataButton = true;
                        printf("[Data Button : %s]\r\n", dataButton.c_str());
                    }
                }
            }
            
            switch(step) {
                case 0: {
                    ping_counter1++;
                    if(ping_counter1 == 10) {
                        int res_cek = socket.send(cek_con,sizeof(cek_con));
                        ping_counter1 = 0;
                        wait(0.05);
                        if(res_cek <= 0) {
                            printf("Server Has Been Closed..\r\n");
                            isConnected = false;
                            socket.close();
                            dsp.init();

                            continue;
                        }
                    }

                    for(uint8_t i = 0; i<board_sum; i++) {
                        //wait_ms(50);
                        Send_RS485(i,64,real_blink_id[i],true);
                        //printf("%d %d %d %d %d\r\n",real_blink_id_1,real_blink_id_2,real_blink_id_3,real_blink_id_4,real_blink_id_5);

                        for(uint8_t a = 4; a<12; a++) {
                            Row_Board_Now[i][a-4] = respon_from_slave[a];
                        }
                    }
                    salah_ambil.clear();
                    salah_taruh.clear();

                    lst_salah.clear();
                    to_remove.clear();
                    blinking_now = 0;
                    for(uint8_t i = 0; i<board_sum; i++) {
                        res_cmp = 0;
                        //printf("Now : ");
//                        for(int v=0; v<(sizeof(Row_Board_Now[i])/sizeof(Row_Board_Now[i][0])); v++) printf("%02X ",Row_Board_Now[i][v]);
//                        printf("\r\n");
//
//                        printf("Last : ");
//                        for(int v=0; v<(sizeof(Row_Board_Last[i])/sizeof(Row_Board_Last[i][0])); v++) printf("%02X ",Row_Board_Last[i][v]);
//                        printf("\r\n");

                        res_cmp = memcmp(Row_Board_Now[i],Row_Board_Last[i], (sizeof(Row_Board_Now[i])/sizeof(Row_Board_Now[i][0])));
                        //dbg.printf("memcmp :%d\r\n",res_cmp);
                        if(res_cmp != 0) {
                            for(uint8_t j = 0; j<8; j++) {
                                if(Row_Board_Now[i][j] != Row_Board_Last[i][j]) {

                                    keys = 0;
                                    bin_string_now = Byte_to_StringBinary(Row_Board_Now[i][j]);
                                    bin_string_predict = Byte_to_StringBinary(Row_Board_Last[i][j]);

                                    for (uint8_t k = 0; k < 8; k++) {
                                        if(bin_string_now[abs(k-7)] != bin_string_predict[abs(k-7)]) {
                                            keys = (40*j)+k+(8*i)+1;
                                            done[i] = false;
                                            lst_salah.push_back(keys);
                                            //printf("Salah : %d",keys);
                                            if(bin_string_predict[abs(k-7)] == '0') {
                                                salah_taruh.push_back(keys);
                                            } else {
                                                salah_ambil.push_back(keys);
                                            }
                                        }
                                    }
                                }
                            }
                        } else if (res_cmp == 0) {
                            real_blink_id[i] = 64;
                            done[i] = true;

                            //memcpy(Row_Board1_Last,Row_Board1_Now,sizeof(Row_Board1_Last));

                            Send_RS485(i,64,64,false);
                        }
                    }

                    salah_ambil_count = 0;
                    salah_taruh_count = 0;

                    if(lst_salah.size() > 0) {

                        int blink_id = 64;

                        bool ada = false;

                        if(lst_salah_hst.size() > 0) {
                            //printf("jumlah hst = %d\r\n",lst_salah_hst.size());
                            for(int q = 0; q<lst_salah_hst.size(); q++) {
                                ada = false;
                                //printf("lst_hst_recent%d = %d\r\n",q+1,lst_salah_hst[q]);

                                for(int r = 0; r<lst_salah.size(); r++) {
                                    if(lst_salah[r] == lst_salah_hst[q]) {
                                        ada = true;
                                    }
                                }
                                if(!ada) {
                                    to_remove.push_back(lst_salah_hst[q]);
                                }
                            }

                            if(to_remove.size() > 0) {
                                for(int t = 0; t<to_remove.size(); t++) {
                                    lst_salah_hst.erase(remove(lst_salah_hst.begin(), lst_salah_hst.end(), to_remove[t]), lst_salah_hst.end());
                                }
                            }

                            for(int w = 0; w<lst_salah.size(); w++) {
                                it = find (lst_salah_hst.begin(), lst_salah_hst.end(), lst_salah[w]);
                                if (it != lst_salah_hst.end()) {
                                    continue;
                                }
                                lst_salah_hst.push_back(lst_salah[w]);
                            }
                        } else {
                            for(int u = 0; u<lst_salah.size(); u++) {
                                lst_salah_hst.push_back(lst_salah[u]);
                            }

                        }

                        blink_id = lst_salah_hst[lst_salah_hst.size()-1];

                        int board_number_blink = (blink_id - 1)%40;

                        if(board_number_blink>= 0 && board_number_blink<= 7)board_number_blink = 0;
                        else if(board_number_blink>= 8 && board_number_blink<= 15)board_number_blink = 1;
                        else if(board_number_blink>= 16 && board_number_blink<= 23)board_number_blink = 2;
                        else if(board_number_blink>= 24 && board_number_blink<= 31)board_number_blink = 3;
                        else if(board_number_blink>= 32 && board_number_blink<= 39)board_number_blink = 4;

                        //printf("board_number_blink = %d\r\n",board_number_blink);

                        uint8_t baris_blink = (blink_id - 1)/40;
                        uint8_t kolom_blink = (blink_id - 1)%8;
                        blink_id = board_88[baris_blink][kolom_blink];

                        //printf("Board blink : %d,Kolom blink : %d, Baris blink : %d\r\n",board_number_blink,kolom_blink,baris_blink);
                        for(int i =0; i<board_sum; i++) {
                            if(board_number_blink == i) {
                                real_blink_id[i] = blink_id;
                            } else {
                                real_blink_id[i] = 64;
                            }
                        }
                        blinking_now = lst_salah_hst[lst_salah_hst.size()-1];

                        salah_ambil_count = salah_ambil.size();
                        salah_taruh_count = salah_taruh.size();
                        to_sends.clear();

                        to_sends.push_back(0x02);
                        to_sends.push_back((blinking_now>>8)&0x0FF);
                        to_sends.push_back(blinking_now & 0x0FF);
                        to_sends.push_back((salah_ambil_count>>8)&0x0FF);
                        to_sends.push_back(salah_ambil_count & 0x0FF);
                        to_sends.push_back((salah_taruh_count>>8)&0x0FF);
                        to_sends.push_back(salah_taruh_count&0x0FF);

                        for(int y = 0; y<salah_ambil.size(); y++) {
                            to_sends.push_back((salah_ambil[y] >> 8) & 0x0FF);
                            to_sends.push_back(salah_ambil[y] & 0x0FF);
                        }

                        for(int y = 0; y<salah_taruh.size(); y++) {
                            to_sends.push_back((salah_taruh[y] >> 8) & 0x0FF);
                            to_sends.push_back(salah_taruh[y]  & 0x0FF);
                        }

                        if(to_sends_last != to_sends) {

                            socket.send(to_sends.data(), to_sends.size());
                            to_sends_last.clear();
                            copy(to_sends.begin(), to_sends.end(), back_inserter(to_sends_last));
                            dsp.step5(to_string(blinking_now).c_str(),true);
                        }
                    } else {
                        if(done[0] && done[1] && done[2] && done[3] && done[4]) {
                            lst_salah_hst.clear();
                            blinking_now = 0;
                            memset(real_blink_id,64,(sizeof(real_blink_id) / sizeof(real_blink_id[0])));

                            salah_ambil_count = 0;
                            salah_taruh_count = 0;

                            step = 1;

                            to_sends.clear();
                            to_sends.push_back(0x02);
                            to_sends.push_back(0x00);//blinking now byte1
                            to_sends.push_back(0x00);//blinking now byte2
                            to_sends.push_back(0x00);//salah_ambil byte1
                            to_sends.push_back(0x00);//salah_ambil byte2
                            to_sends.push_back(0x00);//salah_taruh byte1
                            to_sends.push_back(0x00);//salah_taruh byte2

                            if(to_sends_last != to_sends) {
                                socket.send(to_sends.data(), to_sends.size());
                                to_sends_last.clear();
                                copy(to_sends.begin(), to_sends.end(), back_inserter(to_sends_last));

                            }
                            if(frm_data_received.ids == 3) {
                                dsp.step4(to_string(frm_data_received._key_id).c_str(),false);
                            } else if(frm_data_received.ids== 1) {
                                dsp.step0();
                            } else {
                                dsp.step0();
                            }
                            //wait_ms(50);
                            dsp.step5(to_string(blinking_now).c_str(),true);

                        } else {
                            blinking_now = 0;
                            memset(real_blink_id,64,(sizeof(real_blink_id) / sizeof(real_blink_id[0])));
                        }
                    }
                }
                break;

                case 1: {
                    char rbuffer[64];

                    int rcount = socket.recv(rbuffer, sizeof rbuffer);
                    if(rcount < 0) {
                        step = 0;
                    } else if(rcount > 0) {
                        rbuffer[rcount] = '\0';

                        string data;

                        for(int i = 0; i<sizeof(rbuffer); i++) {
                            if(rbuffer[i] == '\0')break;
                            data += rbuffer[i];
                        }

                        vector<string> res = split(data, "~");

                        if(sizeof(res) != 0) {
                            temp_data = split(res[0], ";");

                            if(temp_data[0] == "3" && temp_data[1] == "1") {
                                dsp.step4(to_string(frm_data_received._key_id).c_str(),false);
                            } else if(temp_data[0] == "3" && temp_data[1] == "0") {
                                dsp.step0();
                            } else {
                                frm_data_received.ids = atoi(temp_data[0].c_str());
                                frm_data_received.ibutton = temp_data[1];
                                frm_data_received._key_id = atoi(temp_data[2].c_str());
                                frm_data_received.cmd_id = atoi(temp_data[3].c_str());


                                printf("id = %d\r\n",frm_data_received.ids);
                                printf("ibutton = %s\r\n",frm_data_received.ibutton.c_str());
                                printf("keys = %d\r\n",frm_data_received._key_id);
                                printf("cmd = %d\r\n",frm_data_received.cmd_id);

                                if(frm_data_received.ids== 1) {
                                    wait(1);
                                    dsp.step1(true);
                                    step = 2;
                                } else if(frm_data_received.ids == 3) {
                                    step = 3;

                                    //dsp.step4(to_string(frm_data_received._key_id).c_str(),true);
                                }
                            }
                        }
                    } else if(rcount == 0) {
                        printf("Server Has Been Closed..\r\n");

                        isConnected = false;
                        step = 0;//
//                        blinking_now = 0;
//                        real_blink_id_1 = 64;
//                        real_blink_id_2 = 64;
//                        real_blink_id_3 = 64;
//                        real_blink_id_4 = 64;
//                        real_blink_id_5 = 64;
                        dsp.init();
                        socket.close();
                    }

                    if(ibutton_code != "") {
                        string data_to_send = "5;" + ibutton_code + ";~";
                        printf("%s\r\n",data_to_send.c_str());
                        wait(1);

                        socket.send(data_to_send.c_str(), data_to_send.length());
                        printf("Send ibutton\r\n");

                        ibutton_code.clear();
                    }
                }
                break;

                case 2: {
                    buzz.play(1);
                    bool restart = false;
                    while(frm_data_received.ibutton.c_str() != ibutton_code) {
                        wait(0.05);
                        ping_counter2++;

                        if(ping_counter2==25) {
                            int res_cek = socket.send(cek_con,sizeof(cek_con));
                            ping_counter2 = 0;
                            wait(0.05);
                            if(res_cek <= 0) {
                                printf("Server Has Been Closed..\r\n");
                                isConnected = false;
                                socket.close();
                                dsp.init();
                                restart = true;

                                break;
                            }
                        }
                    }
                    if(restart) {
                        continue;
                    } else {
                        ibutton_code.clear();
                        printf("IButton verified\r\n");
                        dsp.step2(to_string(frm_data_received._key_id).c_str());
                        step = 3;

                    }
                }
                break;

                case 3: {
                    ping_counter3++;

                    if(ping_counter3 == 10) {
                        int res_cek = socket.send(cek_con,sizeof(cek_con));
                        ping_counter3 = 0;
                        wait(0.05);
                        if(res_cek <= 0) {
                            printf("Server Has Been Closed..\r\n");
                            isConnected = false;
                            socket.close();
                            dsp.init();
                            continue;
                        }
                    }

                    buzz.play(1);

                    board_number = (frm_data_received._key_id - 1)%40;

                    if(board_number>= 0 && board_number<= 7)board_number = 0;
                    if(board_number>= 8 && board_number<= 15)board_number = 1;
                    if(board_number>= 16 && board_number<= 23)board_number = 2;
                    if(board_number>= 24 && board_number<= 31)board_number = 3;
                    if(board_number>= 32 && board_number<= 39)board_number = 4;
                    printf("KEY[board] = %d\r\n",board_number);
                    baris = (frm_data_received._key_id - 1)/40;
                    printf("KEY[baris] = %d\r\n",baris);
                    kolom = (frm_data_received._key_id - 1)%8;
                    printf("KEY[kolom] = %d\r\n",kolom);
                    idle_wait_result = 0;
                    real_id = board_88[baris][kolom];
                    printf("Real Id = %d\r\n",real_id);

                    Make_Prediction(board_number,baris,kolom);

                    step = 4;

                }
                break;

                case 4: {
                    ping_counter4++;

                    if(ping_counter4 == 10) {
                        int res_cek = socket.send(cek_con,sizeof(cek_con));
                        ping_counter4 = 0;
                        wait(0.05);
                        if(res_cek <= 0) {
                            LCD_Step_4 = 0;
                            printf("Server Has Been Closed..\r\n");
                            isConnected = false;
                            socket.close();
                            dsp.init();
                            continue;
                        }

                    }

                    for(uint8_t i = 0; i<board_sum; i++) {
                        wait_ms(100);
                        if(board_number == i) {
                            Send_RS485(i,real_id,real_blink_id[i],true);
                        } else {
                            Send_RS485(i,64,real_blink_id[i],true);
                        }

                        printf("Data has been received successfully\r\n");
                        for(uint8_t a = 4; a<12; a++) {
                            Row_Board_Now[i][a-4] = respon_from_slave[a];
                            //printf("Resp : %02X || BoardLast %d : %02X\r\n",respon_from_slave[a],i+1,Row_Board2_Last[temp_id-5]);
                        }
                    }

                    salah_ambil.clear();
                    salah_taruh.clear();

                    lst_salah.clear();

                    for(uint8_t i = 0; i<board_sum; i++) {
                        res_cmp = 0;
                        res_cmp = memcmp(Row_Board_Now[i],Row_Board_Predict[i], (sizeof(Row_Board_Now[i])/sizeof(Row_Board_Now[i][0])));
                        if(res_cmp != 0) {
                            for(uint8_t j = 0; j<8; j++) {
                                if(Row_Board_Now[i][j] != Row_Board_Predict[i][j]) {
                                    keys = 0;
                                    bin_string_now = Byte_to_StringBinary(Row_Board_Now[i][j]);
                                    bin_string_predict = Byte_to_StringBinary(Row_Board_Predict[i][j]);

                                    for (uint8_t k = 0; k < 8; k++) {
                                        if(bin_string_now[abs(k-7)] != bin_string_predict[abs(k-7)]) {
                                            keys = (40*j)+k+(8*i)+1;
                                            if(keys != frm_data_received._key_id) {
                                                lst_salah.push_back(keys);
                                                if(bin_string_predict[abs(k-7)] == '0') {
                                                    salah_taruh.push_back(keys);
                                                } else if(bin_string_predict[abs(k-7)] == '1') {
                                                    salah_ambil.push_back(keys);
                                                }
                                            } else {
                                                done[i] = false;
                                            }
                                        }
                                    }
                                }
                            }
                        } else if (res_cmp == 0) {
                            printf("Board %d Match\r\n",i+1);
                            real_blink_id[i] = 64;
                            done[i] = true;

                            memcpy(Row_Board_Last[i],Row_Board_Now[i],(sizeof(Row_Board_Last[i])/sizeof(Row_Board_Last[i][0])));

                            Send_RS485(i,64,64,false);
                        }
                    }

                    salah_ambil_count = 0;
                    salah_taruh_count = 0;

                    //printf("Size salah = %d\r\n",lst_salah.size());
                    if(lst_salah.size() > 0) {
                        int  blink_id = 64;

                        bool ada = false;
                        //for(int p = 0; p<lst_salah.size(); p++) printf("lst salah all%d = %d\r\n",p+1,lst_salah[p]);

                        if(lst_salah_hst.size() != 0) {

                            //printf("jumlah hst = %d\r\n",lst_salah_hst.size());
                            for(int q = 0; q<lst_salah_hst.size(); q++) {
                                ada = false;

                                for(int r = 0; r<lst_salah.size(); r++) {
                                    if(lst_salah[r] == lst_salah_hst[q]) {
                                        ada = true;
                                        //break;
                                    }
                                }
                                if(!ada) {
                                    to_remove.push_back(lst_salah_hst[q]);
                                }
                            }

                            if(to_remove.size() > 0) {
                                for(int t = 0; t<to_remove.size(); t++) {
                                    lst_salah_hst.erase(remove(lst_salah_hst.begin(), lst_salah_hst.end(), to_remove[t]), lst_salah_hst.end());
                                }
                            }

                            for(int w = 0; w<lst_salah.size(); w++) {
                                it = find (lst_salah_hst.begin(), lst_salah_hst.end(), lst_salah[w]);
                                if (it != lst_salah_hst.end()) {
                                    continue;
                                }
                                lst_salah_hst.push_back(lst_salah[w]);
                            }
                        } else {

                            for(int u = 0; u<lst_salah.size(); u++) {
                                lst_salah_hst.push_back(lst_salah[u]);
                            }
                        }

                        blink_id = lst_salah_hst[lst_salah_hst.size()-1];
                        //printf("blink_id = %d\r\n",blink_id);

                        int board_number_blink = (blink_id - 1)%40;

                        if(board_number_blink>= 0 && board_number_blink<= 7)board_number_blink = 0;
                        else if(board_number_blink>= 8 && board_number_blink<= 15)board_number_blink = 1;
                        else if(board_number_blink>= 16 && board_number_blink<= 23)board_number_blink = 2;
                        else if(board_number_blink>= 24 && board_number_blink<= 31)board_number_blink = 3;
                        else if(board_number_blink>= 32 && board_number_blink<= 39)board_number_blink = 4;

                        //printf("board_number_blink = %d\r\n",board_number_blink);

                        int baris_blink = (blink_id - 1)/40;
                        int kolom_blink = (blink_id - 1)%8;

                        blink_id = board_88[baris_blink][kolom_blink];

                        //printf("Board blink : %d,Kolom blink : %d, Baris blink : %d\r\n",board_number_blink,kolom_blink,baris_blink);
                        for(int i =0; i<board_sum; i++) {
                            if(board_number_blink == i) {
                                real_blink_id[i] = blink_id;
                            } else {
                                real_blink_id[i] = 64;
                            }
                        }

                        blinking_now = lst_salah_hst[lst_salah_hst.size()-1];

                        salah_ambil_count = salah_ambil.size();
                        salah_taruh_count = salah_taruh.size();
                        to_sends.clear();
                        to_sends.push_back(frm_data_received.ids);
                        to_sends.push_back((blinking_now>>8)&0x0FF);
                        to_sends.push_back(blinking_now & 0x0FF);
                        to_sends.push_back((salah_ambil_count>>8)&0x0FF);
                        to_sends.push_back(salah_ambil_count & 0x0FF);
                        to_sends.push_back((salah_taruh_count>>8)&0x0FF);
                        to_sends.push_back((salah_taruh_count>>8)&0x0FF);

                        for(int y = 0; y<salah_ambil.size(); y++) {
                            to_sends.push_back((salah_ambil[y] >> 8) & 0x0FF);
                            to_sends.push_back(salah_ambil[y] & 0x0FF);
                        }

                        for(int y = 0; y<salah_taruh.size(); y++) {
                            to_sends.push_back((salah_taruh[y] >> 8) & 0x0FF);
                            to_sends.push_back(salah_taruh[y]  & 0x0FF);
                        }

                        if(to_sends_last != to_sends) {
                            socket.send(to_sends.data(), to_sends.size());
                            to_sends_last.clear();
                            copy(to_sends.begin(), to_sends.end(), back_inserter(to_sends_last));

                            dsp.step5(to_string(blinking_now).c_str(),true);

                            LCD_Step_4 = 0;
                        }
                    } else {
                        if(done[0] && done[1] && done[2] && done[3] && done[4]) {
                            lst_salah_hst.clear();
                            blinking_now = 0;
                            memset(real_blink_id,64,(sizeof(real_blink_id) / sizeof(real_blink_id[0])));

                            salah_ambil_count = 0;
                            salah_taruh_count = 0;

                            ibutton_code.clear();

                            to_sends.clear();
                            to_sends.push_back(frm_data_received.ids);
                            to_sends.push_back(0x00);//blinking_now byte1
                            to_sends.push_back(0x00);//blinking_now byte2
                            to_sends.push_back(0x00);//salah_ambil byte1
                            to_sends.push_back(0x00);//salah_ambil byte2
                            to_sends.push_back(0x00);//salah_taruh byte1
                            to_sends.push_back(0x00);//salah_taruh byte2

                            if(to_sends_last != to_sends) {
                                socket.send(to_sends.data(), to_sends.size());
                                to_sends_last.clear();
                                copy(to_sends.begin(), to_sends.end(), back_inserter(to_sends_last));
                            }
                            ibutton_code.clear();
                            to_sends.clear();
                            LCD_Step_4 = 0;

                            step = 0;

                            if(frm_data_received.ids == 3) {
                                dsp.step4(to_string(frm_data_received._key_id).c_str(),false);
                            } else if(frm_data_received.ids == 1) {
                                dsp.step0();
                            }
                        } else {

                            char rbuffer[64];

                            int rcount = socket.recv(rbuffer, sizeof rbuffer);
                            if(rcount < 0) {

                            } else if(rcount > 0) {
                                rbuffer[rcount] = '\0';

                                string data;

                                for(int i = 0; i<sizeof(rbuffer); i++) {
                                    if(rbuffer[i] == '\0')break;
                                    data += rbuffer[i];
                                }

                                vector<string> res = split(data, "~");

                                if(sizeof(res) != 0) {
                                    temp_data = split(res[0], ";");

                                    if(temp_data[0] == "1" && temp_data[1] == "0") {
                                        ibutton_code.clear();
                                        to_sends.clear();
                                        LCD_Step_4 = 0;
                                        step = 0;
                                        dsp.step0();
                                        continue;
                                    }
                                }
                            } else if(rcount == 0) {
                                printf("Server Has Been Closed..\r\n");

                                isConnected = false;
                                step = 0;//
                                dsp.init();
                                socket.close();
                            }

                            memset(real_blink_id,64,(sizeof(real_blink_id)/sizeof(real_blink_id[0])));
                            blinking_now = 0;

                            idle_wait_result++;

                            if(idle_wait_result == 200) {
                                to_sends.clear();
                                to_sends.push_back(frm_data_received.ids);
                                to_sends.push_back((real_id>>8)&0x0FF);
                                to_sends.push_back(real_id & 0x0FF);
                                to_sends.push_back(0x00);//salah_ambil byte1
                                to_sends.push_back(0x00);//salah_ambil byte2
                                to_sends.push_back(0x00);//salah_taruh byte1
                                to_sends.push_back(0x00);//salah_taruh byte2

                                if(to_sends_last != to_sends) {
                                    socket.send(to_sends.data(), to_sends.size());
                                    to_sends_last.clear();
                                    copy(to_sends.begin(), to_sends.end(), back_inserter(to_sends_last));
                                }
                            }

                            if(LCD_Step_4 == 0) {
                                if(frm_data_received.ids== 1) {
                                    dsp.step2(to_string(frm_data_received._key_id).c_str());
                                    wait(1);
                                    LCD_Step_4 = 1;
                                } else if(frm_data_received.ids == 3) {
                                    dsp.step4(to_string(frm_data_received._key_id).c_str(),true);
                                    wait(1);
                                    LCD_Step_4 = 1;
                                }
                            }
                        }
                    }

                }
                break;
            }
        }
    }
}

int main()
{
    //serial_rx_tick.attach(&RS485_Recv,0.01);
    for(int i = 0; i<5; i++) {
        memset(Row_Board_Last[i],0,(sizeof(Row_Board_Last[i])/sizeof(Row_Board_Last[i][0])));
        memset(Row_Board_Now[i],0,(sizeof(Row_Board_Now[i])/sizeof(Row_Board_Now[i][0])));
        memset(Row_Board_Predict[i],0,(sizeof(Row_Board_Predict[i])/sizeof(Row_Board_Predict[i][0])));
    }

    ids = 0;
    to_sends_last.clear();
    to_sends_last.push_back(2);
    to_sends_last.push_back(0x00);//blinking now byte1
    to_sends_last.push_back(0x00);//blinking now byte2
    to_sends_last.push_back(0x00);//salah_ambil byte1
    to_sends_last.push_back(0x00);//salah_ambil byte2
    to_sends_last.push_back(0x00);//salah_taruh byte1
    to_sends_last.push_back(0x00);//salah_taruh byte2

    wait(0.01);
    dsp.init();

//    thd_Cek_Ibutton.start(Cek_Ibutton);
    thd_Buzzer.start(Cek_Salah_Buzzer);
    //thd_LCD.start(Cek_LCD_State);
    //thd_RS485_Recv.start(RS485_Recv);

    FindingServer();
}

string Byte_to_StringBinary(byte data_byte)
{
    string data_temp;
    for(int i = 0; i <8; i++) {
        if((data_byte>>i)&0x01) {
            data_temp[abs(i-7)] += '1';
        } else data_temp[abs(i-7)] += '0';
    }
    return data_temp;
}

uint8_t StringBinary_to_Byte(string data_binary)
{
    uint8_t data_temp = 0;
    bitset<8> b(data_binary);
    unsigned char c = ( b.to_ulong() & 0xFF);
    data_temp = static_cast<uint8_t>(c);

    return data_temp;
}

int Get_Row(int key_id)
{
    int data_temp = 0;

    data_temp = key_id/(board_sum*8);

    return data_temp;
}

unsigned short Get_CRC_Modbus(byte data_byte[])
{
    unsigned short data_temp;

    data_temp = calculate_crc16_Modbus(reinterpret_cast<char*>(data_byte), 6);

    return data_temp;
}
byte to_send[6] = {0x00,0x03,0x14,0x15,0x01,0x00};
void Send_RS485(int board,int key_id,int key_id_blinking,bool Cek_Respon)
{
    to_send[0] = board+1;
    to_send[2] = key_id;
    to_send[3] = key_id_blinking;
    //printf("Calculating crc : ");
    unsigned short __crc = Get_CRC_Modbus(to_send);

    //printf("%02X\r\n",__crc);
    memcpy(buffer_crc,(unsigned char*)&(__crc),sizeof(short));
    byte _eof = 0x0D;

    byte all_to_send[9];

    all_to_send[0] = board+1;
    all_to_send[1] = to_send[1];
    all_to_send[2] = to_send[2];
    all_to_send[3] = to_send[3];
    all_to_send[4] = to_send[4];
    all_to_send[5] = to_send[5];
    all_to_send[6] = buffer_crc[1];
    all_to_send[7] = buffer_crc[0];
    all_to_send[8] = _eof;

Resend:

//    printf("Data sent : ");
//    for(int i = 0; i<9; i++) {
//        printf("%02X ",all_to_send[i]);
//    }
//    printf("\r\n");

    printf("Data recv : ");
    for(int i = 0; i<15; i++) {
        printf("%02X ",respon_from_slave[i]);
    }
    printf("\r\n");
    wait(0.05);
    rede = 1;
    for(int i = 0; i<9; i++) {
        dv.putc(all_to_send[i]);
    }

    if(Cek_Respon) printf("Send Complete to %d\r\n",board+1);

    if(!Cek_Respon) {
        intr_Done = true;
    } else {
        //printf("Trying to received%d..\r\n",board+1);
        memset(respon_from_slave, 0, (sizeof respon_from_slave/sizeof respon_from_slave[0]));
        intr_Done = false;
        ids = 0;
        rede = 0;

        while(!intr_Done) {
            while(dv.readable()) {

                char tempbuf = dv.getc();
                if(ids == 0 && tempbuf == 0xFF) {

                } else {
                    respon_from_slave[ids] = tempbuf;
                    ids++;
                }
            }

            //            printf("Data recv : ");
//            for(int i = 0; i<17; i++) {
//                printf("%02X ",respon_from_slave[i]);
//            }
//            printf("\r\n");
            //printf("Trying to checking..\r\n");

            if((respon_from_slave[0] == board+1) && (respon_from_slave[14] == 0x0D)) intr_Done = true;
            else goto Resend;
        }
    }
}


void Cek_Ibutton()
{
//    while(1) {
//        
//    }
}

void Cek_Salah_Buzzer()
{
    while(1) {
        if(real_blink_id[0] != 64 || real_blink_id[1] != 64 || real_blink_id[2] != 64 || real_blink_id[3] != 64|| real_blink_id[4] != 64) {
            buzz.play(3);
        }
        wait(0.2);
    }
}

string frames;
void First_Update()
{
    frames.clear();

    for(uint8_t i = 0; i<board_sum; i++) {
        wait_ms(50);
        intr_Done = false;
        ids = 0;
        Send_RS485(i,64,64,true);
        printf("Send Done\r\n");
        for(uint8_t a = 4; a<12; a++) {
            Row_Board_Last[i][a-4] = respon_from_slave[a];
            //printf("%02X\r\n",Row_Board1_Last[temp_id-5]);
        }
    }

    string bin_string;
    salah_ambil.clear();
    salah_taruh.clear();
    string key_ids;
    for (uint8_t i = 0; i < board_sum; i++) {
        for (uint8_t j = 0; j < 8; j++) {
            bin_string = Byte_to_StringBinary(Row_Board_Last[i][j]);
            //printf("board[%d][%d] = %s\r\n",i,j,bin_string.c_str());
            for (uint8_t k = 0; k < 8; k++) {
                if (bin_string[abs(k-7)] == '1') {
                    salah_ambil.push_back((40*j)+k+(8*i)+1);//Salah Ambil -> Key_On
                }
            }
        }
    }

    to_sends.clear();
    to_sends.push_back(0x04);
    to_sends.push_back(0x00);
    to_sends.push_back(0x00);
    to_sends.push_back((salah_ambil.size()>>8)&0x0FF);
    to_sends.push_back(salah_ambil.size() & 0x0FF);
    to_sends.push_back((salah_taruh.size()>>8)&0x0FF);
    to_sends.push_back(salah_taruh.size()&0x0FF);

    for(int y = 0; y<salah_ambil.size(); y++) {
        to_sends.push_back((salah_ambil[y] >> 8) & 0x0FF);
        to_sends.push_back(salah_ambil[y]  & 0x0FF);
    }

    for(int y = 0; y<salah_taruh.size(); y++) {
        to_sends.push_back((salah_taruh[y] >> 8) & 0x0FF);
        to_sends.push_back(salah_taruh[y] & 0x0FF);
    }
}
string data_bin_pred;
void Make_Prediction(int board_no,int baris, int kolom)
{
    printf("baris = %d\r\n",baris);
    printf("kolom = %d\r\n",kolom);

    for(int i = 0; i<board_sum; i++) {
        memcpy(Row_Board_Predict[i],Row_Board_Last[i],(sizeof(Row_Board_Predict[i])/sizeof (Row_Board_Predict[i][0])));
        if(board_no == i) {
            printf("Predict Before: ");
            for(int j = 0; j<8; j++) {
                printf("%02X ",Row_Board_Predict[i][j]);
            }
            printf("\r\n");
            data_bin_pred = Byte_to_StringBinary(Row_Board_Predict[i][baris]);
            printf("Before %s\r\n",data_bin_pred.c_str());

            if(frm_data_received.cmd_id == 1) {
                data_bin_pred[abs(kolom-7)] = '0';
            } else if(frm_data_received.cmd_id == 2) {
                data_bin_pred[abs(kolom-7)] = '1';
            }
            printf("After %s\r\n",data_bin_pred.c_str());
            uint8_t datas = stoi(data_bin_pred,0,2);
            Row_Board_Predict[i][baris] = datas;
            printf("Predict After: ");
            for(int i = 0; i<8; i++) {
                printf("%02X ",Row_Board_Predict[i][i]);
            }
            printf("\r\n");

        }
    }
}