#include <stdio.h>
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <sys/socket.h>
#include <bluetooth/bluetooth.h>
#include <bluetooth/hci.h>
#include <bluetooth/hci_lib.h>
#include <bluetooth/rfcomm.h>

bdaddr_t find_client(char *target) {
    inquiry_info *ii = NULL;
    int max_rsp, num_rsp;
    int dev_id, sock, len, flags;
    int i;
    char addr[19] = { 0 };
    char name[248] = { 0 };
    bdaddr_t target_channel = {0};

    dev_id = hci_get_route(NULL);
    sock = hci_open_dev( dev_id );
    if (dev_id < 0 || sock < 0) {
        perror("opening socket");
        exit(1);
    }

    len  = 8;
    max_rsp = 255;
    flags = IREQ_CACHE_FLUSH;
    ii = (inquiry_info*)malloc(max_rsp * sizeof(inquiry_info));
    
    num_rsp = hci_inquiry(dev_id, len, max_rsp, NULL, &ii, flags);
    if( num_rsp < 0 ) perror("hci_inquiry");

    for (i = 0; i < num_rsp; i++) {
        target_channel = (ii+i)->bdaddr;
        ba2str(&target_channel, addr);
        memset(name, 0, sizeof(name));
        if (hci_read_remote_name(sock, &(ii+i)->bdaddr, sizeof(name), 
                                 name, 0) < 0)
            strcpy(name, "[unknown]");
        int j = strlen(name);
        while (name[j-1] == ' ' || name[j-1] == '\n' || name[j-1] == '\r')
            name[--j] = '\0';
        if (strcmp(name, target))
            printf("skipped:  %s  %s\n", addr, name);
        else {
            printf("find:     %s  %s\n", addr, name);
            break;
        }
    }

    free( ii );
    close( sock );
    return target_channel;
}

int main(int argc, char **argv)
{
    struct sockaddr_rc addr = {
        AF_BLUETOOTH, // rc_family
        find_client("dronermctrl"), // rc_bdaddr
        (uint8_t) 1 // rc_channel
    };
    int s, status, bytes_read, buf_end = 0, i;
    char buf[2048] = {0}, bak[2048] = {0};

    // allocate a socket
    s = socket(AF_BLUETOOTH, SOCK_STREAM, BTPROTO_RFCOMM);

    // connect to server
    status = connect(s, (struct sockaddr *)&addr, sizeof(addr));
    if( status < 0 )
        perror("connect failed");
    
    while (1) {
        bytes_read = read(s, buf + buf_end, sizeof(buf));
        buf_end += bytes_read;
        for (i = buf_end - bytes_read; i < buf_end; ++i)
            if (buf[i] == '\n') {
                buf[i-1] = '\0';
                printf("received: [[%s]]\n", buf);
                memcpy(bak, buf, sizeof(buf));
                memset(buf, 0, sizeof(buf));
                memcpy(buf, bak + i + 1, sizeof(buf) - i - 1);
                buf_end -= i + 1;
            }
    }

    close(s);
    return 0;
}